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ABSTRACT 



A sequential extended Kalman filter and optimal 
smoothing algorithm was developed to provide real time 
estimates of torpedo position and depth on the three 
dimensional underwater tracking range at the Naval Torpedo 
Station, Keyport , Washington- The measurements consisted of 
acoustic pulse transit times from the torpedo to receiving 
array, which are nonlinear functions of the positions and 
the depth of the torpedo, were linearized and filter gains 
and filtered estimates of states calculated- By running the 
smoothing subroutine, all past filtered estimates of states 
and error covariance were smoothed. The program was tested, 
using simulated torpedo trajectories that traversed both 
single and multiple arrays, on an IBM-PC. The results showed 
that filter performance was dependent on system noise and 
the distance to the hydrophone array from the torpedo and 
the smoothed estimates of states and error covariances were 



better than or equal to the filtered estimates. 
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INTRODUCTION 



I. 



The Naval Torpedo Station at Keyport , Washington 
currently operates two three-dimensional underwater tracking 
ranges utilizing a sonar transmitter installed in the 
torpedo to be tracked- The transmitter is synchronized with 
a master clock- Timed acoustic pulses are received by 
hydrophone arrays and then relayed via cable to a computer 
at the observation site. The computer calculates the 
positional coordinates of the torpedo and plots its 
trajectory through the water. 

The measured data, which consist of the elapsed time 
from transmission of a’ pulse until its receipt at the 
hydrophone array, is corrupted with noise due to combined 
effects of environmental factors and measurement 
i nstruments. 

The intention is to implement and test a sequential 
extended Kalman filter and smoothing routine which processes 
the transit times of the acoustic pulses and generates the 
filtered and smoothed estimates of the positions of tracked 
torpedo at a particular time. The design takes into account 
the elimination of the storage problem. 
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II. DESCRIPTION QF RANGE TRACKING GEOMETRY 



The hydrophone array, consisting of four independent 
elements, defines an orthogonal coordinate system in which 
transit time measurements are made. As shown in Figure 2.1, 
four hydrophones X, Y, Z, and C are on four adjacent 
vertices separated * by a distance d, along the edge of the 
cube. The origin of the array coordinates is at the center 
of the cube with the orthogonal coordinates parallel to its 
edge. Positional information is computed from the transit 
times of a periodic synchronous acoustic signal traveling 
from the torpedo to the four hydrophones on the array.The 
.torpedoes are equipped with sonar transmitters which are 
transmitting an acoustic signal in every 1.31 seconds, 
within a range accuracy 3 to 30 ft. When tracking by 
multiple arrays, the signal from the closest hydrophone 
array is defined as the basis for the time measurements and 
for the range calculations. A more detailed description of 
the range tracking capability is described in [Ref. 1, 23. 
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Figure 2.1 Geometry o-f a Tracking Array 



1 6 



III. THEORY 



A. EXTENDED KALMAN FILTER 

The basic idea of the extended Kalman -filter is to 
relinearize about each estimate X(k/k), once it has been 



computed. As soon as a new state estimate is made, a new and 
better reference state trajectory is incorporated into the 
estimation process. CRef. 3, 4, 53 

For the three-dimensional location problem three 
position states (X, Y, Z) and two velocity states (V^ , V ) 



specify target motion. The discrete linear and nonlinear 
observation equations are given by 



X ( k + 1 ) = $ . X < k ) + p . W(k) 



(3.1) 



and 



Z (k) = h (X (k) , k) + V(k) 



(3.2) 



wheres and p are constant matrices; 

h is a nonlinear function of the state X 

W(k) is the plant excitation noise; 

V(k) is the measurement noise. 

In these equations the plant noise and measurement noise are 
assumed uncorrelated (white) with zero mean. That is, 

ECW(k) .W T ( j) 3 = Q* (k) & . 

K J 
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and 



ECV(k) . V T ( j ) 1 = R ( k ) & . 

J 

where: & = 1 , k = j ; 

= O , k ^ j . 

In order to apply the linear -filter, Equation 3.2 is 
expanded in a Taylor series about the best estimate o-f the 
state at that time and only the -first order terms are kept. 
Equation 3.2 gives 

Z (k ) = H ( k ) . X < k ) + V(k) (3.3) 



where 



H ( k ) = — Tr- 



ail 

ax' 



(3.4) 



X (k) = X (k/k - 1) 



X(k/k - 1) is a predicted value o-f the state at time k, 

given the measurements until time k-1. 

A state error vector is defined by 

X (k/k) = X (k/k) - X (k) , 

and a predicted state error vector is defined by 
X (k/k - 1) = X (k/k - 1) - X < k ) . 

The covariance of state error matrix is defined by 
P ( k/k ) = ECX ( k/k ) . X T (k/k)l, 

the predicted covariance of state error matrix is given by 
P ( k/k - 1 ) = ECX (k/k - 1).X T (k/k - 1)]. 
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The state excitation matrix is given by 



Q(k) = p.ECW(k) . W T (k) 3. p T , 



and the measurement noise covariance matrix is 



R(k) = ECV(k) . V (k) 3, 



The Kalman filter equations are given by CRef. 3 , 
P (k-t-l/k ) = 5 P ( k/k ) $ T + Q(k) 

G(k)=P(k/k-l)H T (k)CH(k)P(k/k-l)H T (k)+R(k)3 -1 
P ( k/k ) = Cl - G(k) H ( k ) 1 P(k/k-l) 

X(k+l/k) = 5 X ( k / k ) 

Z (k/k- 1 ) = h(X (k/k- 1 ) , k) 

X (k/k ) = X ( (k/k- 1 ) + G ( k ) CZ(k) - Z(k/k-l)3 



4, 53: 
(3. 5) 



(3.6) 



(3. 7) 



(3.8) 



(3.9) 



(3. 10) 



The Q matrix serves not only to allow for maneuvering 
but also to account for any model inaccuracies, that is, any 
di screpanci es between the true action of the torpedo and its 
char acter i z at i on by Equation 3.1. The Q matrix also serves 
to prevent the gain matrix G(k) from approaching zero by 
always insuring uncertanity in the predicted covariance of 
error matrix P(k+l/k) CRef. 1, 3, 4, 51. 
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B. OPTIMAL SMOOTHING 



Smoothing is a non-real time data processing scheme that 
uses all measurements between 0 and N to estimate the state 
of a system at certain time k, where 0 k X N. The smoothed 
estimate of X(k> based on all measurements between 0 and N 

A 

is denoted by X(k/N). The smoothed error covariance is 

denoted by P(k/N) and P(k/N) P<k/k) means that the 

smoothed estimate of X(k) is at least as good as the 

filtered estimate or equal to its filtered estimate for all 
the time, except the terminal time. This is shown 
graphically in Figure 3.1. As portrayed in Figure 3.2, there 
are three classes of particular interest because of their 
applicability to realistic problems CRef. 3, 4, 51. One is 
the Rauch-Tung— Str i ebel form, which was chosen in our 
particular problem CRef. 6 , 71. 

The smoothed state estimate and the smoothed error 
covariance matrix are given by 

X ( k/N) = X < k/k ) + A(k) CX (k+l/N) - X(k + l/k)l (3.11) 

X ( k + 1 / k ) = 5 X (k/k) ( 3 . 12 ) 

P ( k/N) =P ( k/k ) +A (k) CP(k+l/N) — P (k+l/k) DA(k) T (3.13) 

where 

A ( k ) = P ( k/k ) i T P _1 (k + l/k) for k ,< N. 
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Figure 3.1 Advantage o-f Per-forming Optimal Smoothing 



ui 

il * Htjtf) — ,;r r ) » i it, t , I 

[ i p — -* p- — — j 




Data available from whole interval 



<bl 



mt./f.) 

f 

f 

♦ 



p 



■ Estimates at one time of interest 



v 



f 



1C) 

r 


Growing length of data 


1 


L 

'o 


t, i 


J 

'/ 




p |V intervals - ■ * 




L 


Data 


- 


r 


i(/,. 


1 


L 

f <j 


'..i 


3 

'/ 




p N intervals » 






Data 





Fi gu re 3, 2 



Three types o*f smoothing: (a) fixed-interval 

(b) fixed-point, <c) fixed-lag smoothing- 



IV. PROBLEM DEFINITION 



A, OBSERVATION AND PLANT STATE EQUATIONS 



In the torpedo tracking problem, the non-linear 
observation equations are the four independent transit times 

from the tracked torpedo to the hydrophones, T , T , T and 

c x y 

T . Thus the non— linear measurement matrix is defined by 
2 

Z ( k ) = CT (k) T (k) T (k) T (k)] T + V(k) (4.1) 

- c x y z - 

where 



T (k)=---C (X (k) +d/2) 2 +(Y(k)+d/2) 2 +(Z (k) +d/2) 2 ] 1/2 

c vel 



T ( k ) =---C (X(k)-d/2) 2 + (Y(k) +d/2> 2 + < Z < k ) +d/2) 2 J 1 /2 
x vel 



T ( k ) =— — — C ( X ( k ) +d/2) 2 + ( Y ( k ) — d/2) 2 + ( Z ( k ) +d/2) 2 3* /2 

y vel 



T ( k ) — C ( X ( k ) +d/2) 2 + ( Y ( k ) +d/2) 2 + ( Z ( k ) — d/2 ) 2 ] 1/2 
z vel 



Since the transit times are readily available and 
non-linear functions of position, these equations can be 
linearized and Kalman filter theory applied using the 
extended Kalman filter. This procedure produces a real time 



filtering on the transit times T . T , T and T - without 

c 7 x y 2 

the necessity o-f converting these times to positions. 

Equation 3.4 can be used to give the linearized 
observation matrix. When the derivatives are taken and 

evaluated at the predicted state values X(k/k-l) = X* (k) the 

result is 
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The measurement noises, V(k) 's, are assumed to be 
zero-mean and independent with a covariance matrix 



0 



0 



R (k) = 



• 0 



’T 



0 



The plant state equations are 



X (k + 1 ) 



| X (k) + T V (k) + g . \ 

i X 1 > 



! v <k+i> J 

> X i 



! V (k) + g„ 
: x y 2 



j Y (k+1 ) ! = { Y ( k ) + 



TV (k) 

y 



( 4 . 2 ) 



! v (k+i) J 
• y • 



! v 



(k) + g„ 

y ^4 



! Z (k+1 ) 



! z<k) + g, 



where X(k), Y(k) and Z(k) are the position coordinates of 

the torpedo at time t<k), V <k) and V (k) are the X and Y 

7 x y 



components of the velocity. 
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The excitation terms g^ through g^_ are included to take 

into account the random changes in speed <y ) , heading 

V t 

<y >, and depth ( y 7 ) , which are assumed to be independent, 

9 t <- 

zero mean, rates o-f changes. Typical maneuvering parameters 
-for the torpedo are given in CRe-f. 83. 



ff. = 22 o /sec; 
e t 




ECr e 1 

9 t 



Gq = 36 -ft/sec 
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* 2 = 1 ft / sec; 
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EC r 2 z 3 



The effect of this excitation is to increase the predicted 
covariance of the state error matrix. 

The excitation covariance matrix is given by 



Q 



p.ECW(k) 



W T ( k ) 3 . p"*" 



(4.3) 



and 
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where the states are evaluated at the current state 

A 

estimates X(k/k). Substituting th.ese expressions in the Q 
matrix results in 
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A more detailed derivation of the excitation covariance 
matrix is given in CRef. 81. 

The excitation matrix serves not only to take into 
account the possibility of maneuvering, but of model 
inaccuracies as well. Q also used to prevent the gains of 
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the filter from approaching zero as more and more data is 
processed, by insuring some uncertainty in the predicted 
state values CRef. 3, 4, 53. 

In the state form, the plant state equation is 

X ( k+1 ) = § X ( k ) + p W(k) <4. 4) 



where: 



: i t o o o : 

! o l o o o i 



: T 2 /2 0 0 i 

i t o o i 
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and 



r = '■ 



T 2 /2 o! 



! o o o i o ! 



o 



T oi 



! o o o o i ! 



! o b o! 



B. DEFINITION OF MULTIPLE ARRAY TRACKING 

The coordinate system is de-fined as shown in Figure 4.1. 
These 72 positions, an XYZ position -for each o-f 4 
hydrophones in 6 arrays, are placed into a 6 x 12 matrix 
HYDRO and re-ferenced throughout the program.The torpedo 
position is re-ferenced to a central level rectangular 
coordinate system. The non-linear observation equations 
become 

Z(k) = CT < k ) T (k) T <k) T <k)3 T +• V(k) (4.5) 

- c ;< y z - 

where 



T ( k ) C < X ( k ) —X . 
c vel iC 



2 + <Y<k)-Y.„ 
iC 



2 +(Z(k)-Z. c ) 2 



3 



i /2 
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T < k ) =— — 7 C (X ( k ) — X 
y vel 



2 2 21/2 
. w > +<Y(k)-Y. xy > +(Z(k)-Z. vy ) 1 

l Y l Y i Y 



T ( k ) =— — — C (X <k) -X 
z vel 



2 2 21/2 
i2 ) +<Y(k)-Y iz r +(Z(k)-Z iZ ) 1 



and the subscripted variables X, Y, and Z are the 
coordinates of a particular array being used. 

The decision parameter used to determine the switching 
•from array to array is a straight handoff . If the predicted 



x position, X. . , is greater than 3,000 feet from the 

k + l/k 

array in use, then index is incremented and the next row of 
HYDRO is implememted. This placed into the program the X, Y, 
and Z positions of the hydrophones in the next array. The 
handoff can easily be utilized in real range operations, as 
the transit times from adjacent arrays are present at the 
computer for a particular time slot. 
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a ) Coordinate System for Multiple Array Tracking 
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b ) Hydrophone Array Location Matrix 
Figure 4,1 Geometry of Multiple Array Tracking 
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C. SEQUENTIAL EXTENDED KALMAN FILTER 

In the sequential approach, after modifying the basic 

Kalman filter equations, calculations are performed on each 

of the four independent transit times in the following 

order: T , T , T and T for each 1.31 second time slot, 

c x y z 

Since the four transit times are independent and processed 
sequentially, the covariance of error matrix and the state 
vector - are updated four times during each time slot. Thus 
more accurate estimates of the filter states are achived. 
Modification of the filter equations for the sequential 
approach circumvented the matrix inversion in the gain 
equation. An invalid transit time measurement will result in 
the filter ignoring the update information for that 
particular measurement only. 

The estimate of the states X(k/k), based on one transit 

.•v 

time measurement are used as the prediction X(k/k— 1) for the 

cal cul at i onson the next measur ements. Thus for the first 

time measurement T only the first row of the linearized H 

c 

matrix is calculated and then the first gain column 
corr espondi ng to the first time measurement T^_ is calculated 



by 



p(k/k-i) h; 



T 



G 



1 row 



(4.6) 



i col 



H. 



P< k/k-1) H. 



,T 



+ R. 

1 1 



i r ow 



l row 
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where i — 1 to 4 correspond! ng to the tour measured transit 
times. 

An estimate of the particular observation time is 
calculated by using Equation 3-9 evaluated at the predicted 

.•X 

state X(k/k-l)- The difference between observed transit 

times and the estimated transit times forms the residual 
which is used in the estimate equation 

X . = X ( k/k— 1 ) + G. , CResiduall (4.7) 

- i - icol 



This equation gives an estimate of the states based on one 
of the four time measurements. 

The covariance of error is calculated based on one 
measurement by 



P. 

l 



Cl - G 



i col 



H. IP. « 

irow l—l 



(4.8) 



where: I is identity matrix; 

P. „ is the covariance matrix calculated from the 
l-l 

previous transit time measurement or i f i = 1 , 
the predicted error covariance P(k/k-l). 

Editing erroneous time measurement is achieved by 
implementing a three sigma gate using the covariance of the 
measurent noise (R) and the covariance of the estimation 
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error 



P(k/k).The gate then is written -for each time 



measurement i = 1 to 4: 

gate = 3* CC (P . .maximum) / (4860. ) 2 1 + R..> 1/2 (4.9) 

J J ii 

where j = 1, 3, 5. The gate expands or decreases depending 
on the confidence level of the position estimate and the 
transit time. It the difference between the actual transit 
time received and predicted transit time to a particular 
hydrophone exceeds the gate, the measurement is considered 
unacceptable and the filter gain is set to zero causing the 
filter to ignore the data and take the prediction of the 

states as the estimate X(k/k) = X^k^k— 1). 

Bounding the residual bias error is achieved by making 
comparison between the average of the absolute value of the 
time residuals and the preset threshold. If the average of 
the time residuals exceeds the preset threshold, Q is 
calculated and added to the last updated covariance of error 
matrix P. Then filter reiterates the gain, covariance, and 
state estimate equations for the same time slot. This 
procedure continues until the average of the time residuals 
falls below the preset threshold at which time an acceptable 
state vector estimate has been obtained for the time slot. 



D. OPTIMAL SMOOTHING ALGORITHM 



The smoothing solution starts with the filtered estimate 
at the last point and calculates backward point by point* 
determining the smoothed estimate as a linear combination of 
the filtered estimate at that point an.d the smoothed 

estimate at the previous point CRef. 61 . 

It can be seen from the error covariances that the 
filter has reached *a steady-state condition by the end of 
the forward sweep. As an example, let us enter the backward 

.•X 

sweep at the end point where k = 20. Here we have X (20/20) 

and PC20/20). Since the filter solution at this point is 
conditioned on all the measurement data, it is also the 
smoothed estimate at k = N = 20. We are now ready to compute 
the smoothed estimate one step back at k = 19. From 

Equations 3.11, 3.12 and 3.13 we have 

X ( 19/20) = X ( 19/ 19)+ A( 19) CX (20/20) - X(20/19)J 
stored stored 

XC20/19) = 5 X (19/19) 

stored 

A ( 19) = P ( 19/ 19) $ T P -1 (20/19) 

stored stored 



|X > IX> 



P ( 19/20) 



= P ( 19/19) + A ( 19) CP (20/20) - P(20/19>] A T (19) 

stored stored stored 



and to compute the smoothed estimate two step back at k = 1 



(18/20)= X ( 18/ 18) + A ( 18) CX ( 19/20) - X(19/18): 
stored 



(19/18) = 1 X ( 1 8 / 18) 
stored 



A ( 18 ) = P ( 18/18) I T P 1 (19/18) 
stored stored 



P(ia/ 20 ) = P(18/1B) + A ( 18 ) CP ( 19/20) 

stored 



- P ( 19/ 18) ] A T (18) 
stored 



This procedure continues until the time k reaches to 1 



V . SI MULATI ON RESULTS 



A. MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 

The true trajectory of the torpedo is a straight line 
with a 50 ft/sec velocity toward the origin of hydrophone 
array parallel to X-axis, drawing two tangent circles with 
10 deg/sec turn rate, in the horizantal X-Y plane through a 
multiple array. 

In the first part of this run, the initial position of 
the torpedo is 38000 ft in X, 7000 ft in Y, and 300 ft in Z. 



Fi gur es 


5. 1 


and 


5.2 depict 


the 


f i 1 tered 


and smoothed 


est i mate 


of 


the 


trajectory , 


wi th 


zero initial velocity 


errors 


and 


25 


ft initial position 


errors in 


X and Y. The 


errors 


in 


the 


filtered and 


the 


smoothed 


estimate of 



positions in X, Y and Z are drawn in Figures 5.3, 5.4, 5.5, 
5.6, 5.7 and 5.8. For the Kalman filter, errors ranged 
between -1.2 and 2.6 ft in X, -5.9 and 1.9 ft in Y, 0.1 and 
2.5 ft in Z. After smoothing, the errors occured in smaller 
range, which is, between -1.4 and 2.4 ft in X, -5.0 and 1.9 
ft in Y, 0.1 and 0.7 ft in Z. The diagonal terms of the 
filtered and smoothed error covariance matrices are shown 
pictorially in Figures 5.9, 5.10, 5.11, 5.12, 5.13, 5.14, 
5. 15, 5. 16, 5. 17, 5. 18. 
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In the second part of this run, the initial position of 
the torpedo is 35000 ft in X, 7000 ft in Y, and 300 ft in Z. 
The filtered and smoothed estimate of the trajectory are 
drawn in Figures 5.19 and 5.20. Taking this different 
initial geometry made the errors in the position of the 
torpedo to take place in bigger values during first time 
slot of the filtering and last time slot of the smoothing. 
As seen in Figures 5.21, 5.22, 5.23, 5.24, 5.25 and 5.26, 
errors ranged between —16.3 and 1.9 ft in X, -15.1 and 4.6 
ft in Y, -5.0 and 1.0 ft in Z for the Kalman filter and for 
the smoothing this error range is between -12.6 and 1.3 ft 
in X, -12.3 and 4.8 ft in Y, -1.9 and 1.0 ft in Z. The 
diagonal terms of the filtered and smoothed error covariance 
matrices displayed slightly different magnitude, as seen in 
Figures 5.27, 5.28, 5.29, 5.30, 5.31, 5.32, 5.33, 5.34, 5.35 
and 5 . 36 . 

B. MULTIPLE ARRAY ADAPTIVE STRAIGHT RUN 

In this run, the true trajectory of the torpedo is a 
straight line with a 50 ft/sec velocity toward the origin of 
hydrophone array parallel to X-axis in the horizantal X-Y 
plane through a multiple array. 

With the initial position of the torpedo is 38000 ft in 
X, 7000 ft in Y, and 300 ft in Z. The filtered and smoothed 
estimate of the trajectory, with zero initial velocity 
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are 



errors and 25 ft initial position errors in X and V, 
depicted in Figures 5.37 and 5.38. Figures 5.39, 5.40, 5.41, 
5.42, 5.43 and 5.44 give the errors in the filtered and the 

smoothed estimate of positions in X, V and Z. For the Kalman 
filter, errors ranged between -1.6 and 2.6 ft in X, -5.9 and 
4.7 ft in Y, -0.2 and 2.5 ft in Z. After smoothing, the 
errors occured in smaller range, which is, between —1.1 and 
2.4 ft in X, -5.0 and* 1.7 ft in Y, -0.2 and 0.6 ft in Z. The 
diagonal terms of the filtered and smoothed error covariance 
matrices are shown pictorially in Figures 5.45 through 5.54. 

C. SINGLE ARRAY ADAPTIVE MANEUVERING RUN 

The previous tests described the filter and smoothing 
performance for both straight and maneuvering runs through 
multiple array. Using the same basic torpedo trajectories as 
in multiple array, similar tests are performed for 
maneuvering run through single array.During the single array 
tracking, the initial position of the torpedo is 7500 ft in 
X, 1300 ft in Y and 0 ft in Z, which gives different initial 
geometry. The filtered and smoothed estimates of the 
trajectory and the corr espondi ng position errors in X, Y and 
Z are pictorially given in Figures 5.55 through 5.62. For 
the Kalman filter, errors ranged between —1.6 and 3.3 ft in 
X, -19.1 and 8.9 ft in Y, -0.3 and 1.6 ft in Z. After 
smoothing, the errors occured in smaller range, which is. 
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between -1.1 and 3.1 -ft in X, -17.9 and 5.0 ft in Y, -0.1 
and 1.6 ft in Z. The diagonal terms of the filtered and 
smoothed error covariance matrices are shown pictorially in 
Figures 5.63 through 5.72. 

D. SINGLE ARRAY STRAIGHT RUN 

The purpose of this last series of tests is to 
functionally demonstrate the performance of the filter and 
smoothing during a straight run through single array using 
the same initial torpedo position as in single array 
adaptive maneuvering run. The filtered and smoothed 
estimates of the trajectory and the correspondi ng position 
errors in X, Y and Z are pictorially given in Figures 5.73 
through 5. SO. For the Kalman filter, errors ranged between 
—1.6 and 3.3 ftin X, -19.1 and 8.9 ft in Y, -0.3 and 0.S ft 
in Z. After smoothing, the errors occured in smaller range, 
which is, between -0.6 and 3.1 ft in X, -17.9 and 3.5 ft in 
Y, -0.2 and 0.7 ft in Z. The diagonal terms of the filtered 
and smoothed error covariance matrices are shown pictorially 
in Figures 5.80 through 5.90. 
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VI. CONCLUSIONS 



The sequential extended Kalman filter and smoothing 
routine sufficiently generated the filtered and smoothed 
estimates of the states, which specify the motion of the 
torpedo. Errors generated by running the routine on the 
IBM— PC are comparable to those given in the previous search, 
which was done on * a large IBM computer CRef. 13. 

In the smoothing problem, computing the predicted 

estimates of the states, X(k+l/k), from the estimates of the 

• / V .A. 

states, X(k/k), eliminates the storage problem for X(k+l/k). 

In future studies, an algorithm for computing P(k/k) from 
P(k+l/k+l> and hence eliminating the storage problem for 
P(k/k), should be invesigated. 

Examining the errors and their covariances, it is 
evident that the uncertainty in position exist only in the 
Y direction for the case where the torpedo is moving along 
the X axis. The results of the straight run analyses show 
that the propagation of the filtered error covariance is 
dependent on the path of the torpedo with respect to 
hydrophone array. Upon observing the error propagation it is 
apparent that the position errors exhibit appr ox i mat el y 
equal oscillations about zero indicating that the 
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measurement noise is the dominant error source driving the 
filter. 

The smoothed estimates of the states are at least as 
good as or better than the filtered estimates. The filter 
performance was dependent on system noise and the distance 
from the torpedo to the hydrophone array. Errors get bigger 
as the torpedo approaches the tracking limit of the 
hydrophone array. 

Additional work should be done using trajectories 
generated from actual torpedo runs on the Dabob test range. 
The rotation and reduction of the error ellipsoids should 
be also included in future studies. 

The filter should be of use in range safety in warning 
for possible collisions. Also it may prove invaluable in 
torpedo recovery when there is a malfunction and the torpedo 
is sometimes buried in many feet of mude. 



40 



MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN j 
FILTERED ESTIMATE OF TRAJECTORY WITH NOISE 

O 

o 




XCK/KD (FT) *10* 



Figure 5.1 Filtered Estimate of Trajectory of the Torpedo 
During a Maneuvering Run through Multiple Array 



MULTIPLE ARRAY AOAPTIVE MANEUVERING RUN 
SMOOTHED ESTIMATE OF TRAJECTORY WITH NOISE 



o 

o 

o 




Figure 5.2 Smoothed Estimate of Trajectory of the Torpedo 
During a Maneuvering Run through Multiple Array 
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MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 
ERROR IN FILTERED ESTIMATE WITH NOISE 



o 

o 




Figure 5.3 Error in Filtered Estimate o-f Position in X o-f 
the Torpedo During a Maneuvering Run through Multiple Array 



MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 
ERROR IN SMOOTHED ESTIMATE WITH NOISE 



O 

O 




Figure 5.4 Error in Smoothed Estimate o-f Position in X o-f 
the Torpedo During a Maneuvering Run through Multiple Array 
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MULTIPLE RRRRT RDRPT I V E MRNEUVER I NG RUN 
ERROR IN FILTERED ESTIMATE WITH NOISE 
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Figure 5.5 Error in Filtered Estimate of Position in Y of 
the Torpedo During a Maneuvering Run through Multiple Array 



MULTIPLE RRRAT ADOPTIVE MANEUVERING RUN 
ERROR IN SMOOTHED ESTIMATE WITH NOISE 



O 

O 




Figure 5.6 Error in Smoothed Estimate o-f Position in Y of 
the Torpedo During a Maneuvering Run through Multiple Array 
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MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 
ERROR IN FILTERED ESTIMATE WITH NOISE 




Figure 5-7 Error in Filtered Estimate o-f Position in Z o* 
the Torpedo During a Maneuvering Run through Multiple Array 



MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 
ERROR IN SMOOTHED ESTIMATE WITH NOISE 



o 

PJ 




Figure 5-8 Error in Smoothed Estimate o*f Position in Z of 
the Torpedo During a Maneuvering Run through Multiple Array 



44 



MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 
FILTERED ERROR COVARIANCE P (K/K) WITH NOISE 




Figure 5.9 Variance o-f Filtered Position Error in X o-f the 
Torpedo During a Maneuvering Run through Multiple Array 



MULTIPLE ARRAY AOAPTIVE MANEUVERING RUN 
SMOOTHED ERROR COVARIANCE P (K/NJ WITH NOISE 




Figure 5.10 Variance o-f Smoothed Position Error in X o-f the 
Torpedo During a Maneuvering Run through Multiple Array 
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MULTIPLE ARRAY AOAPTIVE MANEUVERING RUN 
FILTERED ERROR COVARIANCE P (K/KJ WITH NOISE 
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Figure 5.11 Variance of Filtered Velocity Error in X o-f the 
Torpedo During a Maneuvering Run through Multiple Array 



MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 
SMOOTHED ERROR COVARIANCE P (K/N) WITH NOISE 
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Figure 5.12 Variance o-f Smoothed Velocity Error in X o-f the 
Torpedo During a Maneuvering Run through Multiple Array 
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MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 

FILTERED ERROR COVARIANCE P (K/K) WITH NOISE 
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Figure 5.13 Variance of Filtered Position Error in Y of the 
Torpedo During a Maneuvering Run through Multiple Array 



MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 
SMOOTHED ERROR COVARIANCE P (K/N) WITH NOISE 
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Figure 5.14 Variance o-f Smoothed Position Error in Y o-f the 
Torpedo During a Maneuvering Run through Multiple Array 
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MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 
FILTERED ERROR COVARIANCE P (K/KJ WITH NOISE 
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Figure 5.15 Variance o-f Filtered Velocity Error in Y o-f the 
Torpedo During a Maneuvering Run through Multiple Array 



MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN ‘ 
SMOOTHED ERROR COVARIANCE P (K/N) WITH NOISE 
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Figure 5,16 Variance o-f Smoothed Velocity Error in Y o-f the 
Torpedo During a Maneuvering Run through Multiple Array 
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MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 
FILTERED ERROR COVARIANCE P (K/K) WITH NOISE 

O 

o 

o 




Figure 5.17 Variance of Filtered Position Error in Z of the 
Torpedo During a Maneuvering Run through Multiple Array 



MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 
SMOOTHED ERROR COVARIANCE P (K/N) WITH NOISE 
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Figure 5. IS Variance o-f Smoothed Position Error in Z of the 
Torpedo During a Maneuvering Run through Multiple Array 
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MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 
FILTERED ESTIMATE OF TRAJECTORY WITH NOISE 




Figure 5.19 Filtered Estimate o-f Trajectory o-f the Torpedo 
During a Maneuvering Run through Multiple Array 



MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 
SMOOTHED ESTIMATE OF TRAJECTORY WITH NOISE 
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Figure 5.20 Smoothed Estimate o-f Trajectory o-f the Torpedo 
During a Maneuvering Run through Multiple Array 
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MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 
ERROR IN FILTERED ESTIMATE WITH NOISE 
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Figure 5.21 Error in Filtered Estimate o-f Position in X o-f 
the Torpedo During a Maneuvering Run through Multiple Array 



MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 
ERROR IN SMOOTHED ESTIMATE WITH NOISE 
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Figure 5.22 Error in Smoothed Estimate o-f Position in X of 
the Torpedo During a Maneuvering Run through Multiple Array 
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MULTIPLE ARRAY ADAPTIVE' MANEUVERING RUN 
ERROR IN FILTERED ESTIMATE WITH NOISE 
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Figure 5.23 Error in Filtered Estimate o-f Position in Y o-f 
the Torpedo During a Maneuvering Run through Multiple Array 



MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 
ERROR IN SMOOTHED ESTIMATE WITH NOISE 
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Figure 5.24 Error in Smoothed Estimate o-f Position in Y o-f 
the Torpedo During a Maneuvering Run through Multiple Array 
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MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 
ERROR IN FILTERED ESTIMATE WITH NOISE 
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Figure 5.25 Error in Filtered Estimate of Position in 2 of 
the Torpedo During a Maneuvering Run through Multiple Array 



MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 
ERROR IN SMOOTHED ESTIMATE WITH NOISE 
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Figure 5.26 Error in Smoothed Estimate of Position in 2 of 
the Torpedo During a Maneuvering Run through Multiple Array 



MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 
FILTERED ERROR COVARIANCE P (K/K) WITH NOISE 




Figure 5-27 Variance o-f Filtered Position Error in X o-f the 
Torpedo Duri'ng a Maneuvering Run through Multiple Array 



MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 
SMOOTHED ERROR COVARIANCE P (K/N) WITH NOISE 




Figure 5.28 Variance o-f Smoothed Position Error in X o-f the 
Torpedo During a Maneuvering Run through Multiple Array 
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MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 
FILTERED ERROR COVARIANCE P (K/K) WITH NOISE 
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Figure 5.29 Variance o-f Filtered Velocity Error in X of the 
Torpedo During a Maneuvering Run through Multiple Array 



MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 
SMOOTHED ERROR COVARIANCE P (K/N) WITH NOISE 




Figure 5.30 Variance o-f Smoothed Velocity Error in X of the 
Torpedo During a Maneuvering Run through Multiple Array 






MULTIPLE RRRAY ADAPTIVE MANEUVERING RUN 
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Figure 5.31 Variance of Filtered Position Error in Y of the 
Torpedo During a Maneuvering Run through Multiple Array 



MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 
SMOOTHED ERROR COVARIANCE P (K/N) WITH NOISE 




Figure 5.32 Variance of Smoothed Position Error in Y of the 
Torpedo During a Maneuvering Run through Multiple Array 
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MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 
FILTERED ERROR COVARIANCE P (K/K) WITH NOISE 
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Figure 5.33 Variance o-f Filtered Velocity Error in Y o-f the 
Torpedo During a Maneuvering Run through Multiple Array 



MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 
SMOOTHED ERROR COVARIANCE P (K/N) WITH NOISE 
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Figure 5.34 Variance o-f Smoothed Velocity Error in Y o-f the 
Torpedo During a Maneuvering Run through Multiple Array 
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MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 
FILTERED ERROR COVARIANCE P (K/K) WITH NOISE 




Figure 5.35 Variance o-f Filtered Position Error in Z o-f the 
Torpedo During a Maneuvering Run through Multiple Array 




Figure 5.36 Variance o-f Smoothed Position Error in Z o-f the 
Torpedo During a Maneuvering Run through Multiple Array 
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MULTIPLE ARRAY ADAPTIVE STRAIGHT RUN 
Fy_TERED ESTIMATE OF TRAJECTORY WITH NOISE 
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Figure 5.37 Filtered Estimate o-f Trajectory of the Torpedo 
During a Straight Run through Multiple Array 



MULTIPLE ARRAY ADAPTIVE STRAIGHT RUN 
SMOOTHED ESTIMATE OF TRAJECTORY WITH NOISE 
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Figure 5.3S Smoothed Estimate o-f Trajectory o-f the Torpedo 
During a Straight Run through Multiple Array 
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MULTIPLE ARRAY ADAPTIVE STRAIGHT RUN 
ERROR IN FILTEREO ESTIMATE WITH NOISE 
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Figure 5.39 Error in Filtered Estimate o-f Position in X o-f 
the Torpedo During a Straight Run through Multiple Array 



MULTIPLE ARRAY ADAPTIVE STRAIGHT RUN 
ERROR IN SM00THE0 ESTIMATE WITH NOISE 




Figure 5.40 Error in Smoothed Estimate o-f Position in X o-f 
the Torpedo During a Straight Run through Multiple Array 
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MULTIPLE ARRAY HOAPTIVE STRAIGHT RUN 
ERROR IN FILTERED ESTIMATE WITH NOISE 




Figure 5.41 Error in Filtered Estimate o-f Position in Y o-f 
the Torpedo During a Straight Run through Multiple Array 



MULTIPLE ARRAY ADAPTIVE STRAIGHT RUN 
ERROR IN SMOOTHED ESTIMATE WITH NOISE 
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Figure 5.42 Error in Smoothed Estimate o-f Position in Y o-f 
the Torpedo During a Straight Run through Multiple Array 
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MULTIPLE ARRAY ADAPTIVE STRAIGHT RUN 
ERROR IN FILTERED ESTIMATE WITH NOISE 
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Figure 5.43 Error in Filtered Estimate o-f Position in Z of 
the Torpedo During a Straight Run through Multiple Array 




Figure 5.44 Error in Smoothed Estimate of Position in Z of 
the Torpedo During a Straight Run through Multiple Array 
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MULTIPLE- ARRAY AOAPTIVE STRAIGHT RUN 
FILTERED ERROR COVARIANCE P (K/K) WITH NOISE 




Figure 5.45 Variance o-f Filtered Position Error in X o-f the 
Torpedo During a Straight Run through Multiple Array 



MULTIPLE ARRAY AOAPTIVE STRAIGHT RUN 
SMOOTHED ERROR COVRRIANCE P (K/N) WITH NOISE 
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Figure 5.46 Variance of Smoothed Position Error in X o-f the 
Torpedo During a Straight Run through Multiple Array 
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MULTIPLE ARRAY ADAPTIVE STRAIGHT RUN 
FILTERED ERROR COVARIANCE P (K/K) WITH NOISE 
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Figure 5.47 Variance o-f Filtered Velocity Error in X ot the 
Torpedo During a Straight Run through Multiple Array 




Figure 5.48 Variance o-f Smoothed Velocity Error in X o-f the 
Torpedo During a Straight Run through Multiple Array 
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Figure 5.49 Variance o-f Filtered Position Error in Y ot the 
Torpedo During a Straight Run through Multiple Array 
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MULTIPLE ARBAT ADAPTIVE STRAIGHT RUN 
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Figure 5-50 Variance of Smoothed Position Error in Y o-f the 
Torpedo During a Straight Run through Multiple Array 
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MULTIPLE ARRAY ADAPTIVE STRAIGHT RUN 
FILTERED ERROR COVRRIANCE P (K/K) WITH NOISE 
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Figure 5.51 Variance o-f Filtered Velocity Error in V o-f the 
Torpedo During a Straight Run through Multiple Array 



MULTIPLE ARRAY ADAPTIVE STRAIGHT RUN 
SMOOTHED ERROR COVRRIRNCE P (K/N) WITH NOISE 
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Figure 5.52 Variance o-f Smoothed Velocity Error in Y o-f the 
Torpedo During a Straight Run through Multiple Array 
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MULTIPLE ARRAY ADAPTiVE STRAIGHT RUN 
FILTERED ERROR COVRRIRNCE P IK/KJ WITH NOISE 
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Figure 5.53 Variance o-f Filtered 
Torpedo During a Straight Run 



Position Error in Z o-f 
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Figure 5.54 Variance of Smoothed Position Error in 2 of the 
Torpedo During a Straight Run through Multiple Array 
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SINGLE ARRAY ADAPTIVE MANEUVERING RUN 
FILTERED ESTIMATE OF TRAJECTORY WITH NOISE 
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Figure 5-55 Filtered Estimate of Trajectory of the Torpedo 
During a Maneuvering Run through Single Array 



SINGLE ARRAY RDRPTIVE MRNEUVERING RUN 
SMOOTHED ESTIMATE OF TRAJECTORY WITH NOISE 




Figure 5.56 Smoothed Estimate of Trajectory of the Torpedo 
During a Maneuvering Run through Single Array 
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Figure 5.57 Error in Filtered Estimate of Position in X o f 
the Torpedo During a Maneuvering Run through Single Array 



SINGLE ARRAY ADAPTIVE MANEUVERING RUN 
ERROR IN SMOOTHED ESTIMATE WITH NOISE 
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Figure 5.58 Error in Smoothed Estimate of Position in X of 
the Torpedo During a Maneuvering Run through Single Array 
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SINGLE RRRflf RDRPTIVE MANEUVERING RUN 
ERROR IN FILTERED EST I MATE WITH NOISE 
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Figure 5.59 Error in Filtered Estimate o-f Position in Y o-f 
the Torpedo During a Maneuvering Run through Single Array 



SINGLE ARRAY RDRPTIVE MANEUVERING RUN 
ERROR IN SMOOTHED ESTIMATE WITH NOISE 
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Figure 5.60 Error in Smoothed Estimate o-f Position in Y o-f 
the Torpedo During a Maneuvering Run through Single Array 
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SINGLE RRRflT ADAPT I VE MANEUVERING RUN 
ERROR IN FILTERED ESTIMATE WITH NOISE 
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Figure 5.61 Error in Filtered Estimate o-f Position in Z o-f 
the Torpedo During a Maneuvering Run through Single Array 



SINGLE ARRAY ADAPTIVE MANEUVERING RUN 
ERROR IN SMOOTHED ESTIMATE WITH NOISE 
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Figure 5.62 Error in Smoothed Estimate o-f Position in Z o-f 
the Torpedo During a Maneuvering Run through Single Array 
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SINGLE ARRAY ADAPTIVE MANEUVERING RUN 
FILTERED ERROR COVRR I ANCE P (K/K) WITH NOISE 




Figure 5.63 Variance of Filtered Position Error in X of the 
Torpedo During a Maneuvering Run through Single Array 



SINGLE ARRAY AOAPTIVE MANEUVERING RUN 
SMOOTHED ERROR COVARIANCE P tK/N) WITH NOISE 




Figure 5.64 Variance of Smoothed Position Error in X of the 
Torpedo During a Maneuvering Run through Single Array 
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SINGLE ARRAY ADAPTIVE MANEUVERING RUN 
FILTERED ERROR COVARIANCE P IK/K) WITH NOISE 
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Figure 5.65 Variance of Filtered Velocity Error in X of the 
Torpedo During a Maneuvering Run through Sinlge Array 



SINGLE ARRAY ADAPTIVE MANEUVERING RUN 
SMOOTHED ERROR COVARIANCE P (K/N) WITH NOISE 




Figure 5.66 Variance of Smoothed Velocity Error in X of the 
Torpedo During a Maneuvering Run through Single Array 
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SINGLE ARRAY AOAPTIVE MANEUVERING RUN 

FILTERED ERROR COVRRIRNCE P (K/K) WITH NOISE 
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Figure 5.67 Variance o-f Filtered Position Error in Y o-f the 
Torpedo During a Maneuvering Run through Single Array 



SINGLE ARRAY RDRPTIVE MANEUVERING RUN 
SMOOTHED ERROR COVRRIRNCE P (K/N) WITH NOISE 
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Figure 5.6Q Variance o-f Smoothed Position Error in Y of the 
Torpedo During a Maneuvering Run through Single Array 
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SINGLE ARRAY ADAPTIVE MANEUVERING RUN 
FILTERED ERROR COVARIANCE P (K/K) WITH NOISE 
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Figure 5.69 Variance o-f Filtered Velocity Error in Y o-f the 
Torpedo During a Maneuvering Run through Single Array 



SINGLE ARRAT ADAPTIVE MANEUVERING RUN 
SMOOTHED ERROR COVARIANCE P (K/N) WITH NOISE 
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Figure 5.70 Variance o-f Smoothed Velocity Error in Y o-f the 
Torpedo During a Maneuvering Run through Single Array 
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SINGLE ARRAY ADAPTIVE MANEUVERING RUN 
FILTERED ERROR COVARIANCE P CK/KJ WITH NOISE 
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Figure 5.71 Variance o-f Filtered Position Error in Z o-f the 
Torpedo During a Maneuvering Run through Single Array 



SINGLE ARRAY ADAPTIVE MANEUVERING RUN 
SMOOTHED ERROR COVARIANCE P (K/N) WITH NOISE 




Figure 5.72 Variance o-f Smoothed Position Error i n Z o-f the 
Torpedo During a Maneuvering Run through Single Array 
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SINGLE ARRAY AOAPTIVE STRAIGHT RUN 
FJJLTERED ESTIMATE OF TRAJECTORY WITH NOISE 
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Figure 5.73 Filtered Estimate of Trajectory of the Torpedo 
During a Straight Run through Single Array 



SINGLE ARRAY A0APTIVE STRAIGHT RUN 
SMOOTHED ESTIMATE OF TRAJECTORY WITH NOISE 
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Figure, 5. 74 Smoothed Estimate ot Trajectory o-f the Torpedo 
During a Straight Run through Single Array 
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SINGLE ARRAY ADAPTIVE STRAIGHT RUN 
ERROR IN FILTERED ESTIMATE WITH NOISE 




Figure 5.75 Error in Filtered Estimate o-f Position in X o-f 
the Torpedo During a Straight Run through Single Array 



SINGLE ARRAT ADAPTIVE STRAIGHT RUN 
ERROR IN SMOOTHED ESTIMATE WITH NOISE 
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» Figure 5.76 Error in Smoothed Estimate o-f Position in X o-f 
the Torpedo During a Straight Run through Single Array 
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SINGLE ARRAY ADAPTIVE STRAIGHT RUN 
ERROR IN FILTERED ESTIMATE WITH NOISE 
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Figure 5.77 Error in Filtered Estimate of Position in V of 
the Torpedo During a Straight Run through Single Array 



SINGLE ARRAY ADAPTIVE STRAIGHT RUN 
ERROR IN SMOOTHED ESTIMATE WITH NOISE 




Figure 5.78 Error in Smoothed Estimate of Position in Y of 
the Torpedo During a Straight Run through Single Array 
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SINGLE ARRAY ADAPTIVE STRAIGHT RUN 
ERROR IN FILTERED ESTIMATE WITH NOISE 
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Figure 5.79 Error in Filtered Estimate o-f Position in Z o-f 
the Torpedo During a Straight Run through Single Array 



SINGLE ARRAY ADAPTIVE STRAIGHT RUN 
ERROR IN SMOOTHED ESTIMATE WITH NOISE 
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Figure 5.80 Error in Smoothed Estimate o-f Position in Z of 
the Torpedo During a Straight Run through Single Array 



SINGLE ARRAY ADAPTIVE STRAIGHT RUN 
FILTERED ERROR COVARIANCE P (K/K) WITH NOISE 
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Figure 5.81 Variance of Filtered Position Error in X of the 
Torpedo During a Straight Run through Single Array 



SINGLE ARRAY ADAPTIVE STRAIGHT RUN 
SMOOTHED ERROR COVARIANCE P (K/N) WITH NOISE 
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Figure 5.82 Variance of Smoothed Position Error in X of the 
Torpedo During a Straight Run through Single Array 
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SINGLE ARRAY ADAPTIVE STRAIGHT RUN 
FILTERED ERROR COVARIANCE P (K/K) WITH NOISE 
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Figure 5.83 Variance o-f Filtered Velocity Error in X o-f the 
Torpedo During a Straight Run through Single Array 



SINGLE ARRAY ADAPTIVE STRAIGHT RUN 
SMOOTHED ERROR COVARIANCE P (K/N) WITH NOISE 
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Figure 5.84 Variance o-f Smoothed Velocity Error in X o-f the 
Torpedo During a Straight Run through Single Array 
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SINGLE ARRAY AOAPTIVE STRAIGHT RUN 
FILTERED ERROR COVARIANCE P (K/K) WITH NOISE 




Figure 5.85 Variance a-f Filtered Position Error in Y a-f the 
Torpedo During a Straight Run through Single Array 



SINGLE ARRAY AOAPTIVE STRAIGHT RUN 
SMOOTHED ERROR COVARIANCE P (K/N) WITH NOISE 




Figure 5.86 Variance of Smoothed Position Error in Y of the 
Torpedo During a Straight Run through Single Array 
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SINGLE ARRAY ADAPTIVE STRAIGHT RUN 
FILTERED ERROR COVARIANCE P IK/K) WITH NOISE 
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Figure 5.87 Variance o-f Filtered Velocity Error in Y o-f the 
Torpedo During a Straight Run through Single Array 



SINGLE ARRAY ADAPTIVE STRAIGHT RUN 
SMOOTHED ERROR COVARIANCE P (K/N) WITH NOISE 
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Figure 5-88 Variance o-f Smoothed Velocity Error i n Y o-f the 
Torpedo During a Straight Run through Single Array 
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SINGLE ARRAY ADAPTIVE STRAIGHT RUN 
FILTERED ERROR COVARIANCE P IK/K) WITH NOISE 




Figure 5-99 Variance of Filtered Position Error in Z o*f the 
Torpedo During a Straight Run through Single Array 




Figure 5-90 Variance of Smoothed Position Error in Z of the 
Torpedo During a Straight Run through Single Array 
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APPENDIX A 



PROGRAM DESCRIPTION 



A. GENERAL 

The sequential extended Kalman -filter and Smoothing 
routine is described in detail by CRef. 13. Implementation 
is done by using F0RTRAN77 compilers on IBM— PC. CRe-f. 10, 
11, 12, 133. 

B. RUNNING THE PROGRAM ON THE IBM-PC 

These directions apply -for the IBM-PC computer or other 
computers (compati bl es) with two -floopy disk-drives, 640K 
memory, color/graphic board, math coprocessor and paralel 
dot matrix printer or printer/plotter . The software utilized 
during the simulation studies are: 

1. Operating System DOS 2.10 with required -files to 
create virtual disk and -full screen editor 
ut i 1 i ti es. 

2. IBM Professional FORTRAN Compiler 1.00. 

3. Microsoft F0RTRAN77 3.20. 

4. PI ot works PL0TS8. LIB. 

5. Source files. 

Getting the sequential extended Kalman filter and 
smoothing routine started is essentially a five step 
process: start your computer; edit the source file and make 

required changes and then compile; run the executable file 
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and get the data to be available for platting routine; edit 
the source file of plotting routine and make the necessary 
changes for plotting titles and then compile; run platting 
routine. Start the computer up with an operating system and 
get the program running simply by typing "RUN 11 , which is 
given in Appendix E, at promt "A>". 
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APPENDIX B 

SEQUENTIAL EXTENDED KALMAN FILTER AND OPTIMAL SMOOTHING 

PROGRAM LISTING 



PROGRAM THESIS 



REALMS XKKM1 (5) ,PKKM1 (5,5) ,PHI (5,5) , GAMMA (5,3) , GATE 
REAL*8 GAMMAT (3,5) ,C0VW(3,3) ,C0W<4,4) ,QTEMP(5,3) ,P 
REAL*8 TRUX ( 121 ) ,TRUY(121) ,TRUZ(121) ,ZI (4) ,HR0W(5) 
REAL*8 ZHAT , GDENOM , GDTEMP , PDUM (5,5) ,PI (5,5) ,WHTN,A14 
REAL*S ZDIFF (4) ,ZIC(4) ,XI (5) ,XKK(5) ,PKK(5,5) ,ZDIFAV 
REAL*B DATR (17) , WINIT , PHIPKK (5 , 5) ,PKTEMP(5,5> ,SIGACC 
REALMS SIGDI V , SIGCC , XKERR ( 121 ) ,YKERR(121) ,XP6(5,121) 
REAL*8 HYDRO (6, 12) , XB ( 4 ) , YB ( 4 ) , ZB ( 4 ) ,XSERR(121) ,TD(3) 
REALMS SIGCCC , S IGA AC , S IGDDI ,XP(5,121) ,SMTH(121> ,GI (5) 
REAL*8 P5 (121 ,5,5) , SSI (121 ,5,5) , PI (121 ,5,5) ,Q (5, 5) 
REALMS YSERR (121 ) , ZSERR ( 121 ) , GNUM (5) ,PHIT (5,5) , XP1 (5) 
REAL*8 ZD IFTO , CH (5,5) , TEMPI (5,5) , XNNM1 (5) ,TEMP2(5) 
REAL*S AK (5,5) ,AKT(5,5) ,TEMP3(5) ,TEMP4(5,5) ,XKKS(5) 
REAL*8 PNNM1 (5,5) , TEMPS <5, 5) ,TEMP6(5,5) ,PKKS(5,5) 
REAL*8 SS2(5) ,P2(5,5) ,SS3(5,5) ,SS3R(5,5) ,SIG 
REAL *8 ZKERR ( 121 ) , X 1KERR , X2KERR , Y1KERR , Y2KERR , Z'lKERR 
REAL*S Z2KERR , X 1 SERR , X2SERR , Y 1 SERR , Y2SERR , Z 1 SERR 
REAL*8 Z2SERR 

COORDINATES OF HYDROPHONE ARRAY, FOR MULTIPLE ARRAY 

DATA HYDRO/36000. ,30000. ,24000. ,18000. ,12000. ,6000. 

+ , 6*6000 . , 6*0 . 0 , 36030 . , 30030 . , 24030 . , 1 8030 . , 1 2030 . 

+ , 6030 . , 6*6000 . , 6*0 . O , 36000 . , 30000 . , 24000 . , 1 8000 . 

+ , 1 2000 . , 6000 . , 6*6030 . , 6*0 . 0 , 36000 . , 30000 . , 24000 . 

+ , 18000. , 12000. ,7*6000. ,6*30. / 



DATA PKKM1 / 1000. 0,5*0. O, 1000.0,5*0.0,1000.0,5*0. O 
+ ,1000.0,5*0.0,1000.0/ 

DATA PHI/1 . , 4*0. ,1.31,1. , 5*0. , 1 . , 4*0. ,1.31,1. ,5*0. 

+ , 1 . / 
DATA GAMMA /O . 858 ,1.31, 5*0 .0,0.858,1.31, 5*0 .0,1.31/ 
DATA COVW/ 1.0, 3*0.0, 1. 0,3*0. O, 1 . 0/ , WIN IT/O. 49/ 

DATA COW/ 1 . OD-8 , 4*0 .0,1. OD-8 , 4*0 .0,1. OD-8 , 4*0 . 0 
+ , 1 . OD— 8/ 



DATA FOR MULTIPLE ARRAY TRACKING 

DATA DATR/38000. ,7000. ,300. ,-50. ,0. ,3*0. ,3*0. 
+,4.712389, . 1745329, . 1,8.1 ,600. ,800. / 

DATA X KKM 1 / 37975 . O , -50 . 0 , 6975 .0,0.0, 300 . 0/ 
SECOND DATA FOR MULTIPLE ARRAY TRACKING 

DATA DATR/35000. ,7000. ,300. ,-50. ,0. ,3*0. ,3*0. 
+ , 4 . 712389, . 1745329, . 1,8. 1,600. ,800. / 

DATA XKKM1 /34975. 0 , -50.0 ,6975. 0,0.0, 300. 0/ 
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non 



C FIRST DATA FOR SINGLE ARRAY TRACKING 

C DATA DATR/7500. 0 , 1300. 0,0.0, -50 .0,0.0 ,3*0. 0 , 3*0 . 0 

C +,4.712389, . 1745329,0. 1,8.1 ,600.0,800.0/ 

C DATA XKKM1/7475. 0,-50. 0,1275. 0,0. 0,0.0/ 

C SECOND DATA FOR SINGLE ARRAY TRACKING 

C DATA DATR/2000 . 0 , 1000 . 0 , 300 . O , -50 .0,0. 0,3*0. O , 3*0 . 0 

C +,4.712389,. 1745329,0. 1,8.1 ,600.0,800.0/ 

C DATA XKKM1/1975. 0,-50. 0,975. 0,0. 0,300.0/ 

C DATA FOR SUBROUTINE QFIND 

DATA SI GACC/36. 2/ , SI GDI V/ 1.0/ , SIGCC/22. 2/ , NZDI FF/4/ 
DATA 1 1/1/ , I J/2/ , IK/3/, IL/4/ , IM/5/ , MINE/1/ , JTIME/1 19/ 
C OPEN STATEMENTS FOR OUTPUT FILES 
OPEN ( 4 , F I LE= ' TRUD I . DAT ' ) 

OPEN ( 1 3 , F I LE= ' PKK . DAT ' ) 

OPEN ( 1 1 , F I LE= ' PKN . DAT ' ) 

OPEN (7 , FILE= ' XKK. DAT ' ) 

OPEN ( 8 , F I LE= ' XKN . DAT ' ) 

OPEN ( 9 , F I LE= ' XKERR . DAT ' ) 

OPEN ( 10 , FILE* ' XSERR.DAT ' ) 

OPEN ( 12, FILE= 'OUTPUT. DAT ' ) 

C 

SIGCC = (SIGCC * 3.141592654) / 180.0 

C TRANSPOSE OF GAMMA MATRIX 

CALL TRANS ( GAMMA , I M , I K , GAMMAT ) 

C TRANSPOSE OF PHI MATRIX 

CALL TRANS ( PH I , I M , I M , PH I T ) 

USE THESE STATEMENTS TO CALCULATE CONSTANT Q - MATRIX 
CALL PROD ( GAMMA , COVW , I M , I K , I K , QTEMP ) 

CALL PROD < QTEMP , GAMMAT ,IM,IK,IM,Q) 

ITIME = JTIME + 1 
C USE THIS STATEMENT FOR MULTIPLE ARRAY TRACKING 
17 = 0 

C********************************************************** 
C TIME SLOTS START HERE 

DO 128 KK - 1 , ITIME 

WRITE (*,562) KK 

562 FORMAT (/, 10X, 'TIME SLOT IN FILTERING :',I5) 

C 

C CHOSE THE HYDROPHONE ARRAY FOR MULTIPLE ARRAY TRACKING 

IF(XKKM1 (1) .GE. 33000.0) 18 = 1 

IF ( ( XKKM1 (1) .GE. 27000.0) .AND. ( XKKM1 (1) .LT. 33000.0) ) 

+ 18 = 2 

IF ( ( XKKM1 (1) .GE. 21000.0) .AND. (XKKM1 (1) .LT. 27000.0) ) 

+ 18 = 3 

IF ( ( XKKM1 (1) .GE. 15000.0) .AND. ( XKKM1 (1) .LT. 21000.0) ) 

+ 18 = 4 

I F ( (XKKM1 (1) .GE. 9000.0) .AND. ( XKKM1 (1) .LT. 15000.0) ) 

+ 18 = 5 

IF(XKKM1 (1) .LT. 9000.0) 18 = 6 

C 
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207 DO 205 13 = 1 , IL 

14=3* 13 

15 = 14 - 2 

16 = 14 - 1 

XB ( 13) = HYDRO ( IB, 15) 

YB ( 13) = HYDRO (18,16) 

ZB (13) = HYDRO (18,14) 

205 CONTINUE 

C WRITE THE COORDINATES OF CHOSEN HYDROPHONE ARRAY 
IF ( 17. NE. IS) THEN 
WRITE (* ,217) 18, KK 

217 FORMAT (/,1 OX, 'ARRAY ',12 

+ ,' STARTS TRACKING AT TIME ',13) 

WRITE <*, 216) KK, 18, (I3,XB(I3) ,YB<I3) 

+ •, ZB (13), 13 = 1 , IL) 

216 FORMAT (15, 15,4 (T1 1 , I 5 , 3 ( 2X , D 1 4. 8) , / ) ) 

17 = 18 
END IF 



^ ^ ,-N ^ ^ ^ ^ ^ AA AAA/\A AA A AA A AA A.'\A AA A/\ AAAA A A .-X 

C CALCULATE THE TRUE TIMES AND THE TRUE TRAJECTORY 
C USE THIS CALL STATEMENT FOR MULTIPLE ARRAY TRACKING 
CALL TRJC3 (KK , DATR , Z I , TD ,XB,YB,ZB) 

C USE THIS CALL STATEMENT FOR SINGLE ARRAY TRACKING 
C CALL TRAJEC (KK,DATR, ZI ,TD) 

A14 = PHI (1,2) 

TRUX (KK) = TD ( 1 ) 

TRUY (KK) = TD (2) 

TRUZ (KK) = TD (3) 



C 



WRITE (4,306) KK , TRUX (KK) , TRUY (KK) ,TRUZ(KK) 

QAAAAAAAAAAAA/%AAAAAAAAAAAAAAAA/\AAAAAAAAA.^AAAAAAAAAAAAAA.'\AAA 

C ROW OF H - MATRIX 



MINE = 1 

163 DO 132 IROW = 1 , IL 

NZDIFF = 4 

C USE THIS CALL STATEMENT TO RUN NOISE SUBROUTINE 
CALL NOISE (WINIT,WHTN) 

WHTN = ( 1.0 / 3.0 ) * WHTN 
C ZERO NOISE 
C WHTN =0.0 

C USE THIS CALL STATEMENT FOR SINGLE ARRAY TRACKING 
C CALL CHRO W ( IRQW,XKKM1 , HROW ) 

C USE THIS CALL STATEMENT FOR MULTIPLE ARRAY TRACKING 
CALL CHR0W3 ( I ROW , XKKM 1 , HROW , XB , YB , ZB) 
C########################################################## 
CGCK3 = <PCK/K-1 3 (5x5) * HT(5xl) 3 / C H(lx5) 

C * P C K / K - 1 3(5x5) * HT(5xl) + COVE V 3(1) > 

CALL MMULT ( PKKM 1,HR0W,IM,IM, GNUM ) 

CALL VMULT ( HROW , GNUM , I M , GDTEMP ) 
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GDENOM = GDTEMP + COW ( IROW , IROW) 

DO 134 IX = 1 , IM 

GI(IX) = GNUM (IX) / GDENOM 
134 CONTINUE 

CPCK/K3 = < 1(5x5) - G C K 3(5x1) *H (1x5) 3- 
C *PCK/K— 1 3 



DO 135 IP = 1 , IM 

DO 136 JP = 1 , IM 

PDUM ( IP , JP) » < -1. * 61 (IP) ) * HROW(JP) 

IF ( IP. EQ. JP) PDUM(IP,JP) = 1. + PDUM ( IP , JP) 

136 CONTINUE 

135 CONTINUE 

CALL PROD ( PDUM, PKKM1 , IM, IM, IM,PI ) 

C CALCULATE THE PREDICTION OF MEASUREMENTS 
C USE THIS CALL STATEMENT FOR SINGLE ARRAY TRACKING 
C CALL CZHAT ( IROW , XKKM1 , ZHAT) 

C USE THIS CALL STATEMENT FOR MULTIPLE ARRAY TRACKING 
CALL CZHAT3 ( IROW , XKKM1 , ZHAT , XB , YB , ZB) 

ZIC(IROW) = ZI(IROW) + WHTN * O.OOOOl 
ZD IFF ( IROW) = Z IC ( IROW) - ZHAT 
C THREE SIGMA GATE 

P=DMAX 1 (DABS (PI (1,1)) , DABS (PI (3,3) > , DABS (PI (5,5) > ) 
SIG=DSQRT( (P/ ( (4860. ) **2 ) ) + (DABS (COW ( IROW , IROW) ) ) ) 
GATE = 3.0 * SIG 



C 



IF (KK. LE. 4) GO TO 149 

IF (DABS (ZD IFF ( IROW) ) . LT. GATE) GO TO 149 



C 



WRITE (*, 147) KK, IROW, GATE 

147 FORMAT (//,10X, 'THREE SIGMA GATE HAS BEEN EXCEEDED' 

+ , ' AT TIME ',14,' IN ROW ',12,' GATE : *,D14.S) 

DO 148 LGJ = 1 , IM 

GI (LGJ) =0.0 

148 CONTINUE 

C TAG INVALID TIME MEASUREMENT 
ZD IFF ( IROW) = 999. 



C 

C<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 

CXCK/K3=XCK/K— 1 3 + G C K 3 * <Z C K 3 
C -ZCK/K-13J. 

149 DO 150 I = 1 , IM 

XI (I) = XKKMl(I) + GI(I) * ZDIFF ( IROW) 

150 CONTINUE 

C<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 

IF ( IROW. EQ. 4) GO TO 152 
DO 153 I = 1 , IM 

XKKM1 (I) = XI (I) 

153 CONTINUE 
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DO 155 I = 1 , IM 

DO 154 J = 1 , IM 

PKKM1 < I ,J) = PI (I,J) 

154 CONTINUE 

155 CONTINUE 
132 CONTINUE 



152 



I = 1, IM 



DO 156 
XKK ( I ) = XI (I) 

XKKM1 (I) = XI (I) 

C USE THIS ADDITIONAL STATEMENT FOR SMOOTHING 
XP6 ( I , KK) = XI (I) 



C USE 



DO 157 J = 1 , IM 

PKK ( I , J) = P I ( I , J ) 
PKKM1 (I , J) = PI (I ,J) 
THIS ADDITIONAL STATEMENT 
P5(KK, I , J) = PI ( I , J) 



FOR SMOOTHING 



C 

C 

C 



USE 



USE 



157 CONTINUE 
156 CONTINUE 

C>>>>>> >>>>>>>>>>>>>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>>>>>>>>>>> 

C PREDICTION OF MEASUREMENTS BASED ON XCK/K] 

DO 15B I = 1 , IL 

C EDIT INVALID TIME MEASUREMENTS 

IF (ZD IFF ( I ) . GE. 999. ) GO TO 159 
THIS CALL STATEMENT FOR SINGLE ARRAY TRACKING 
CALL CZHAT ( I , XKKM1 , ZHAT) 

THIS CALL STATEMENT FOR MULTIPLE ARRAY TRACKING 
CALL CZHAT3 < I , XKKM1 , ZHAT , XB , YB , ZB) 

ZD IFF ( I ) = DABS ( Z IC ( I ) - ZHAT) 

GO TO 1 58 
ZD IFF ( I ) = 0. 

NZD IFF = NZDIFF -1 

158 CONTINUE 

C > > > > > > > » > > > > > > > > > > > > > >>>>>>>> > > > > > >>>>>> > > > > > > > > > > > > > > > > > 

C ABSOLUTE AVERAGE VALUE OF THE DIFFERENCES IN MEASUREMENT 
IF (NZDIFF. ED. 0) GO TO 160 

ZDIFTO = DABS ( ZD IFF ( 1 ) + ZD IFF (2) + ZD IFF (3) + ZD IFF (4) ) 
ZDIFAV = ZDIFTO / NZDIFF 
IF (KK. LE. 4) GO TO 160 
I F (MINE. EQ. 1 ) SMTH (KK) = ZDIFAV 
IF (MINE. GT. 3) GO TO 160 
C USE THIS CONSTANT (2.0D-6) GATE 

IF ( ZDIFAV. LT . 2. OD— 6) GO TO 160 



159 



WRITE <*, 1473) KK 

1473 FORMAT <//,l OX, 'CONSTANT GATE HAS BEEN EXCEEDED AT' 

+ , ' TIME ' , 1 44 

C ((((((((((((((((((((((((((((((((((((((((((((<(((((((((((( ( 

C USE THESE STATEMENT TO INCREASE THE GAIN IN MULTIPLE 



92 



n n 



C SINGLE ARRAY TRACKING 

SIGAAC = 3.0 * SIGACC 
SIGDDI = 3.0 * SIGDIV 
SIGCCC = 3.0 * SIGCC 



USE THIS CALL STATEMENT TO CALCULATE ADAPTIVE Q-MATRIX 
CALL QF IND(KK,XKK,PKK, SIGAAC, S IGDDI , SIGCCC, A1 4, Q) 
CALL ADD ( PKK , Q , I M , I M , PKKM 1 ) 

MINE = MINE + i 
GO TO 163 

C ((((((((((((((((((((((((((((((((((((((((((((((((((((((((( ( 
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MINE = 1 
NZDIFF = 



>01 



WRITE (7 , 301 ) KK , ( XKK ( J ) , J = 1 , IM) 
WRITE ( 13 , 30 1 ) KK, <PKK(I , I) , I = 1 , IM) 

FORMAT < 15 , 5 (4X , D14 . 9) ) 



XKERR (KK) = 
YKERR(KK) = 
ZKERR(KK) = 



XKK < 1 ) - TRUX (KK) 
XKK <3) - TRUY <KK) 
XKK (5) - TRUZ (KK) 



WRITE (9,306) KK, XKERR (KK) , YKERR ( KK ) , ZKERR (KK ) 
>06 FORMAT ( 15 , 3 (4X , D1 4. 8) ) 

DETERMINE MAX & MIN ERRORS AND THE TIME SLOTS 
IF(KK.EQ.l) THEN 



KX1K = 


KK 


KX2K = 


KK 


KY1K = 


KK 


KY2K = 


KK 


KZ IK = 


KK 


KZ2K = 


KK 


XI KERR 


= XKERR (KK) 


X2KERR 


= XKERR (KK) 


Y1KERR 


= YKERR (KK) 


Y2KERR 


= YKERR (KK) 


Z1KERR 


= ZKERR (KK) 


Z2KERR 


= ZKERR (KK) 



END IF 

IF (XKERR (KK) 
IF ( XKERR (KK) 
IF (XKERR (KK) 
IF ( XKERR (KK) 
IF (YKERR (KK) 
IF (YKERR (KK) 
IF (YKERR (KK) 
IF (YKERR (KK) 
IF (ZKERR (KK) 
IF (ZKERR (KK) 
IF (ZKERR (KK) 



GT. X1KERR) 
GT. X1KERR) 
LT. X2KERR) 
LT. X2KERR ) 
GT. Y1KERR) 
GT. Y1KERR) 
LT. Y2KERR) 
LT. Y2KERR) 
GT. Z1KERR) 
GT. Z1KERR) 
LT. Z2KERR ) 



IF (ZKERR (KK) .LT. Z2KERR) 



KX IK = 


KK 


XI KERR 


= XKERR (KK) 


KX2K = 


KK 


X2KERR 


= XKERR (KK) 


KY IK = 


KK 


Y1KERR 


= YKERR (KK) 


KY2K = 


KK 


Y2KERR 


= YKERR (KK) 


KZ IK = 


KK 


Z1KERR 


= ZKERR (KK) 


KZ2K = 


KK 


Z2KERR 


= ZKERR (KK) 
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C) ))))))))>)))))>>)))>))))>)))))))))))))))>>)>)>)))>))>>>> ) 
CPCK+l/K 3=<PHI(5x5) * P C K / K 3(5x5) * PHIT(5x5>> 

C + Q C ' K 3 

C USE THIS CALL STATEMENT TO CALCULATE ADAPTIVE Q-MATRIX 
CALL QFIND (KK, XKK , PKK , SIGACC , SIGD I V , S IGCC , A 1 4 , Q) 

CALL PROD (PHI , PKK, IM, IM, IM,PHIPKK) 

CALL PROD ( PH I PKK , PH IT, IM, IM, IM , PKTEMP ) 

CALL ADD ( PKTEMP , Q , I M , I M , PKKM 1 ) 

C 

CALL MMULT ( PH I , X KK , I M , IM,XKKM1) 

C) )))))))))))))))))))))))))))))))))))))))))))))))))))))))) ) 
C USE THESE STATEMENTS FOR SMOOTHING 
DO 302 IG = 1 , IM 

XP ( IG,KK) = XKK ( IG) 

302 CONTINUE 

DO 303 III = 1 , IM 
DO 304 JJJ = 1 , IM 

SSI (KK, 1 1 1 , JJJ) = PKKM 1 (III, JJJ) 

PI (KK, III, JJJ) = PKK(III,JJJ) 

304 CONTINUE 

303 CONTINUE 

C SMOOTHING STARTS HERE 

IF (KK. LE. JTIME) GO TO 128 
DO 500 K = 1 , JTIME 

KI = JTIME - K + 1 
WRITE (•* , 561 ) KI 

561 FORMAT (/,10X, 'IN SMOOTHING AT TIME :',I5) 

DO 501 I = 1 , IM 
XP1 (I) = XP6 ( I ,KI ) 

501 CONTINUE 

DO 502 1=1 , IM 

DO 503 J = 1 , IM 

P2 ( I , J ) = P5 (KI , I , J ) 

SS3 ( I , J ) = SSI (KI , I , J) 

IF (KI . LE. 4) GO TO 503 

IF (SMTH (KI ) . GE. 2. OD-6) SS3(I,J) = 3.6 * SS3(I,J) 

503 CONTINUE 

502 CONTINUE 

C****************************************-* -**■*-**•«■ •*-*-**-*-*•*■#•-«•■*-* 
C A (K) = P (K/K) * TRANSPOSECPHI 3 * INVCP (K+l /K) 3 

CALL TRANS (PHI , IM, IM,PHIT) 

CALL REC I P ( SS3 , I M , SS3R ) 

CALL PROD (SS3,SS3R, IM, IM, IM,CH) 

CALL PROD ( PH IT, SS3R , IM, IM, IM, TEMPI) 

CALL PROD (P2, TEMPI , IM, IM, IM,AK) 

C X ( K / N ) = X (K/K) + A (K) * C X(K+1/N) - X(K+1/K) 3 

DO 504 I = 1 , IM 

XNNM1 (I) = XP ( I ,KI+1 ) 

504 CONTINUE 
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CALL MMULT (PHI ,XP1 , IM, IM,SS2) 

CALL SUB ( XNNM1 , SS2 , IM , 1 , TEMP2) 
CALL PROD ( AK , TEMP2 , I M , I M , 1 , TEMP3 ) 
CALL ADD <XP1 , TEMP3, IM, 1 , XKKS) 

DO 505 I = 1 , IM 
XP(I,KI) = XKKS(I) 

505 CONTINUE 



WRITE ( 8 , 30 1 ) KI , (XKKS(J) , J = 1 



IM) 



XSERR(KI) = 
YSERR(KI) = 
ZSERR(KI) = 



XKKS ( 1 ) - TRUX (KI) 
XKKS (3) - TRUY (KI ) 
XKKS (5) - TRUZ (KI ) 



WRITE( 10,306) KI , XSERR (KI ) , YSERR (KI ) , ZSERR (KI ) 
DETERMINE MAX & MIN ERRORS AND THE TIME SLOTS 
IF(K.EQ.l) THEN 



KX IS = 


KI 


KX2S = 


KI 


KYIS = 


KI 


KY2S = 


KI 


KZ IS = 


KI 


KZ2S = 


KI 


X1SERR 


= XSERR (KI) 


X2SERR 


= XSERR (KI) 


Y1SERR 


= YSERR (KI) 


Y2SERR 


= YSERR (KI) 


Z1SERR 


= ZSERR (KI) 


Z2SERR 


= ZSERR (KI) 



END IF 

IF (XSERR (KI) . GT. X1SERR) 
IF (XSERR (K I) . GT. X 1SERR) 
IF (XSERR (KI ) . LT. X2SERR) 
IF (XSERR (KI) . LT.X2SERR) 
IF (YSERR (K I) . GT. Y1SERR) 
IF (YSERR (KI) 

IF (YSERR (KI) 

IF (YSERR (KI) 

IF (ZSERR (KI) 

IF ( ZSERR (KI ) .GT.Z1SERR) 
IF ( ZSERR (KI) -LT. Z2SERR) 
IF ( ZSERR (KI) .LT. Z2SERR ) 



,GT. Y1SERR) 
LT. Y2SERR) 
. LT. Y2SERR ) 
GT. Z1SERR) 



KX 1 S = 


KI 


X1SERR 


= XSERR (K I) 


KX2S = 


KI 


X2SERR 


= XSERR (K I) 


KYIS = 


KI 


Y1SERR 


= YSERR (K I) 


KY2S = 


KI 


Y2SERR 


- YSERR (K I) 


KZ 1 S = 


KI 


Z1SERR 


= ZSERR (KI) 


KZ2S = 


KI 


Z2SERR 


= ZSERR (KI) 



C P (K/N) = P(K/K)+A(K)*CP(K+1/N)-P(K+1/K) 3*TRANSP0SEC A (K) 3 
DO 506 I ■ 1 , IM 

DO 507 J = 1 , IM 

PNNM1 ( I , J ) = PI (KI+1 , I , J ) 

507 CONTINUE 

506 CONTINUE 

CALL SUB ( PNNM 1 , SS3 , I M , I M , TEMP4 ) 

CALL TRANS (AK, IM, IM,AKT) 

CALL PROD (TEMP4, AKT, IM, IM, IM,TEMP5) 

CALL PROD (AK, TEMPS, IM, IM, IM,TEMP6) 
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CALL ADD (P2 , TEMP6 , Ili , IM , PKKS) 

DO 508 I = 1 , IM 

DO 509 J = 1 , IM 

PI <KI , I ,J> = PKKS (I ,J) 

509 CONTINUE 

508 CONTINUE 

C££S£SSS£S£££££$SSSS$£££SS££SSS$SSSS££SSSS£$£SSSSS£S$££££S£ 
WRITE (11, 301) K I , ( PKKS (1,1) , I = 1 , IM) 
C********************************************-***"**** ******* 
500 CONTINUE 

C########################################################## 
128 CONTINUE 

WRITE ( 12,800) 

800 FORMAT (10X,' TIME',4X,' MAX. ERROR ',4X,' TIME',4X 

+ ,' MIN. ERROR ') 

WR I TE ( 1 2 , 80 1 ) KX 1 K , X 1 KERR , KX 2K , X2KERR , K Y 1 K , Y 1 KERR , 
+KY2K , Y2KERR , KZ IK , Z 1KERR , KZ2K , Z2K.ERR 
WR I TE( 12,801) KX IS , X 1SERR , KX2S , X2SERR , KYIS , Y1SERR , 
+KY2S , Y2SERR , KZ IS , Z 1SERR , KZ2S , Z2SERR 

801 FORMAT (3(/,10X,I5,4X,D14.8,4X,I5,4X,D14.8) ) 

STOP 

END 

C 

SUBROUTINE TRANS ( AA , NR , NC , BB ) 

REAL*8 AA(NR,NC) ,BB(NC,NR) 

DO 3 I = 1 , NR 
DO 30 J = 1 , NC 

BB ( J , I ) = AA ( I , J) 

30 CONTINUE 

3 CONTINUE 
RETURN 
END 

C 

SUBROUT I NE PROD ( AA , BB , NR A , NCA , NCB , CC) 

REAL*8 AA ( NRA , NCA ) , BB ( NCA , NCB ) , CC ( NRA , NCB ) 

DO 4 I = 1 , NRA 

DO 40 J = 1 , NCB 

CC(I ,J) =0.0 

40 CONTINUE 

4 CONTINUE 

DO 41 1=1 , NRA 

DO 410 J = 1 , NCB 
DO 411 K = 1 , NCA 

CC(I,J) = CC(I,J) + A A ( I , K) * BB (K , J ) 

411 CONTINUE 
410 CONTINUE 

41 CONTINUE 
RETURN 
END 

C 

SUBROUTINE TRAJEC (KK , DATR , Z I , TD) 
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REAL*8 DATR (17) ,ZI (4) ,TD<3> , COEFF , RANGE , VEL , T 
DATA VEL/4860. 0/ , I IK/3/ , I IM/5/ 

T = 0.0 

COEFF = 1.0 / VEL 

ZI (1)=C0EFF*DSQRT( < ( DATR (1)+15.0) #* 2 ) 

-*■ + ( (DATR(2)+15.0)**2)+( (DATR (3) + 15. O) **2) ) 

ZI (2) =COEFF*DSQRT( < (DATR ( 1 ) -15. 0) **2> 

+ + ( (DATR(2)+15.0)**2>+( (DATR (3) +15. 0) **2> ) 

ZI (3) =COEFF*DSQRT ( ( (DATR ( 1 ) +15. 0) **2> 

+ +( ( DATR ( 2 ) - 1 5 . O ) ** 2 . ) + ( (DATR (3) +15. 0) **2) ) 

ZI (4)=C0EFF*DSQRT( ( (DATR ( 1 ) +15. 0) **2) 

+ -*-( (DATR (2) +15. 0) **2) + ( (DATR (3>-15.0> ** 2 .) ) 

DO 5 I = 1 , I IK 

TD ( I > = DATR ( I.) 

5 CONTINUE 

C USE THIS STATEMENT FOR STRAIGHT RUN 

C IF ( (KK. LE. DATR (17) ) . AND. (KK. GT. DATR ( 16) ) ) GO TO 50 

C USE THESE STATEMENTS FOR MANEUVERING RUN 
58 IF( (KK.LE.49) . AND. (KK.GT.22) ) GO TO 50 
IF( (KK.LE.98) .AND. (KK.GT.71) > GO TO 50 
IF ( (KK. EQ. 50) .OR. (KK.EQ.99) ) THEN 
C 

C FIRST DATA FOR TRUE TRAJECTORY IN SINGLE ARRAY TRACKING 
C DATR (2) = 1300.0 

C DATR (3) =0.0 

C 

C SECOND DATA FOR TRUE TRAJECTORY IN SINGLE ARRAY TACKING 



DATR (2) 


= 1000.0 


DATR (3) 


= 300.0 


DATR (4) 


= -50.0 


DATR (5) 


= 0.0 


DATR (6) 


= 0.0 


DATR (7) 


= 0.0 


DATR (8) 


= 0.0 


DATR (9) 


= 0.0 


DATR ( 10) 


= 0.0 


DATR (11) 


= 0.0 


DATR ( 12) 


= 4.712389 


DATR ( 13) 


= 0. 1745329 


DATR ( 14) 


=0. 1 


DATR ( 15) 


= 8. 1 



END IF 

57 DATR (7) =0.0 
DATR (8) = 0.0 

DATR (14) = 1.31 
GO TO 51 

50 DATR (14) = 0.005 

53 DATR (12) = DATR (12) + DATR (13) * DATR (14) 
DATR (7) = DATR (15) * DCOS (DATR ( 12) ) 

DATR (8) = DATR (15) * DSIN (DATR ( 12) ) 
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DO 52 I = 1 , IIM 

DATR(I) = DATR < I ) + DATR (I +3) * DATR (14) 

b + ( ( (DATR ( 14) ) **2) /2) 

CONT I NUE 
T =. T + DATR ( 14) 

IF (DABS <T - 1.31) .LE. O.OOOl) RETURN 

GO TO 53 

END 



* DATR (1+6) 



SUBROUTINE TRJC3 <KK , DATR , Z I , TD , XB , YB , ZB) 

REALMS DATR (17) , ZI (4) ,TD (3) , XB (4) , YB (4) , ZB (4) ,COEFF 
REAL*8 VEL , T 

DATA VEL/4860. 0/ , I IK/3/ , I IL/4/ , I IM/5/ 

T = 0.0 

COEFF = 1.0 / VEL 
DO 12 I = 1 , TIL 

ZI(I) = COEFF * DSQRT ( ( (DATR ( 1 ) - XB(I))**2) 

+ + ((DATR (2) - YB(I))**2) + ((DATR (3) - ZB(I)>**2>> 

CONTINUE 

DO 120 1=1, I IK 
TD ( I ) = DATR (I) 

CONTINUE 

THIS STATEMENT FOR STRAIGHT RUN 
I F ( ( KK . LE . DATR (17) ) . AND . ( KK . GT . DATR (16))) GO TO 121 
C USE THESE STATEMENTS FOR MANEUVERING RUN 

T. 22) ) GO TO 121 
T. 71) ) GO TO 121 
.99)) THEN 



12 



120 
C USE 



C 128 


IF ( (KK. LE 


.49) .AND. (KK 


C 


I F ( (KK. LE 


. 98) . AND. (KK 


C 


IF ( (KK. EQ 


!.50) .OR. (KK. 


C 


DATR (2) 


= 7000.0 


c 


DATR (3) 


= 300.0 


c 


DATR (4) 


= -50.0 


c 


DATR (5) 


= 0.0 


c 


DATR (6) 


= 0.0 


c 


DATR (7) 


= 0.0 


c 


DATR (8) 


= 0.0 


c 


DATR (9) 


= 0.0 


c 


DATR (10) 


= 0.0 


c 


DATR (11) 


= 0.0 


c 


DATR ( 12) 


= 4.712389 


c 


DATR (13) 


= 0. 1745329 


c 


DATR ( 14) 


= 0.1 


c 


DATR (15) 


= 8. 1 


c 


END IF 




c 






127 


DATR (7) = 


0.0 




DATR (8) = 


0.0 



121 

124 



DATR (14) = 1.31 

GO TO 122 

DATR (14) = 0.005 

DATR (12) = DATR (12) + DATR (13) * DATR (14) 
DATR (7) = DATR (15) * DCOS (DATR ( 12) ) 
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DATR < 8 > = DATR (15) * DSIN (DATR ( 12) ) 

122 DO 123 1=1 , I IM 

DATR ( I ) = DATR ( I ) + DATR(I+3> * DATR (14) 

+ + ( ( <DATR< 14) ) **2> /2> * DATR(I+6) 

123 CONTINUE 

T = T + DATR (14) 

IF (DABS <T - 1.31) -LE. O.OOOl) RETURN 

GO TO 124 

END 

SUBROUTINE CHROW ( I ROW , XKKM1 ,HROW> 

REAL*8 HROW ( 5) , XKKM1 ( 5) , COEFF , DENOM , DEN0M1 , DEN0M2 
REAL*8 VEL , A 1 , A2 , A3 , DEN0M3 , DEN0M4 
DATA VEL/ 4860.0./ 

COEFF = 1.0 / VEL 

DENOM 1 =DSQRT ( ( ( XKKM1 ( 1 ) +15. 0) **2) + ( ( XKKM1 (3) +15. 0) **2> 
+ + ( ( XKKM1 (5) +15.0) **2) ) 

DEN0M2=DSQRT ( ( ( XKKM1 ( 1 ) -15. 0 ) **2) + ( (XKKM1 (3) +15. O) **2 ) 
+ +( (XKKMl <5)+15.0)**2> > 

DEN0M3=DS0RT ( ( (XKKMl ( 1 ) +15. 0) **2) + ( (XKKMl (3) -15.0)**2) 
+ +( (XKKMl (5>+lS.0>**2) > 

DEN0M4=DSQRT ( ( (XKKMl ( 1 ) +15. 0) **2> + ( (XKKMl (3) +15. 0) **2) 
+ +( (XKKMl (5) -15.0) **2> ) 

A1 = 1.0 
A2 = 1.0 
A3 = 1.0 
DENOM = DENOM 1 

IF ( I ROW. EO. 2) DENOM = DEN0M2 
IF( IR0W.EQ.3) DENOM = DEN0M3 
IF( I ROW. ED. 4) DENOM = DEN0M4 



IF ( IROW. EQ. 2) A1 = 
HROW ( 1 ) = COEFF * 


- 1.0 
( (XKKMl (1) 


+ 


A1 


* 


15.0) 


/ 


DENOM) 


IF ( IROW. EQ. 3) A2 = 
HROW (3) = COEFF * 


- 1.0 

( (XKKMl (3) 


+ 


A2 


* 


15.0) 


/ 


DENOM) 


IF ( IROW. EQ. 4) A3 = 
HROW (5) = COEFF * 


- 1.0 
( (XKKMl (5) 


+ 


A3 


* 


15.0) 


/ 


DENOM) 



HROW (2) = 0.0 
HROW ( 4 ) = 0.0 
RETURN 
END 

SUBROUTINE CHR0W3 ( I ROW , XKKMl , HROW , XB , YB , ZB) 

REAL*8 HROW (5) , XKKMl (5) , COEFF , DENOM , VEL , XB (4) ,YB(4) 
REAL*8 X0,Y0,Z0,ZB(4) 

DATA VEL/4860. 0/ 

COEFF = 1.0 / VEL 
XO = XB ( I ROW) 

YO = YB(IROW) 

ZO = ZB( IROW) 

DENOM = DSQRT( ( (XKKMl ( 1 ) -XO) **2> + ( (XKKMl (3)-Y0)**2) 

+ + ( (XKKMl (5) -ZO) **2) ) 
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HROW ( 1 ) 
HROW (2) 


— 


COEFF 

0.0 


* 


( (XKKMl ( 1 ) 


- XO) 


/ 


DENOM) 


HROW (3) 
HROW (4) 




COEFF 

0 . 0 


* 


( (XKKMl (3) 


- YO) 


/ 


DENOM) 


HROW (5) 

RETURN 

END 




COEFF 


* 


( (XKKMl (5) 


- ZO) 


/ 


DENOM) 



c 

SUBROUT I NE MMULT ( AA , BB , NRA , NCA , CC ) 

REAL *8 AA ( NR A , NCA ) , BB ( NCA > , CC < NRA ) 

DO 6 I = 1 , NRA 
CC(I) =0.0 
DO 60 J = 1 NCA 
CC(I> = CC(I> + AA ( I , J ) * BB ( J ) 

60 CONTINUE 

6 CONTINUE 
RETURN 
END 

C 

SUBROUTINE VMULT ( AA , BB , NE , CC) 

REAL*8 AA (NE) ,BB<NE) ,CC 
CC = 0.0 
DO 7 I = 1 , NE 

CC = CC + AA(I) * BB(I) 

7 CONTINUE 
RETURN 
END 

C 

SUBROUT I NE CZHAT ( I ROW , XKKM 1 , ZHAT ) 

REAL*8 XKKM1 (5) , ZHAT , COEFF , VEL 
DATA VEL/4860. 0/ 

COEFF = 1.0 / VEL 

IF ( IROW. EQ. 1) ZHAT=CQEFF*DSQRT ( ( (XKKMl < 1 ) +15. O) **2) + 
+ ( (XKKMl (3)+15.0>**2)+< (XKKMl (5) +15. 0) **2) ) 

IF ( IROW. EQ. 2) ZHAT=COEFF*DSQRT ( ( (XKKMl ( 1 ) -15. 0) *-*2) + 
+ ( (XKKMl (3) +15.0) **2) + ( (XKKMl (5) +15. 0) **2) ) 

IF ( IROW. EQ. 3) ZHAT=COEFF*DSQRT ( ( (XKKMl ( 1 ) +15. 0) **2) + 
+ ( (XKKMl ( 3 ) — 1 5 . 0 ) ** 2 ) + ( (XKKMl (5) +15. O) **2) ) 

IF ( IROW. EQ. 4) ZHAT=COEFF*DSQRT ( ( (XKKMl (1)+15. 0) ** 2 ) + 
+ ( (XKKMl (3) +15. 0) **2) + ( (XKKMl (5) -15. O) **2) ) 

RETURN 
END 
C 

SUBROUTINE CZHAT3 ( IROW , XKKMl , ZHAT , XB , VB , ZB ) 

REAL*8 XKKMl (5) , ZHAT , COEFF , VEL , XB ( 4) ,YB(4) ,ZB(4> , XO 
REAL+8 YO,ZO 
DATA VEL/4860. 0/ 

COEFF = 1.0 / VEL 
XO = XB ( IROW) 

YO = YB ( IROW) 

ZO = ZB (IROW) 
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ZHAT 



= COEFF * DSQRT ( ( ( XKKM1 ( 1 ) - X0)**2) + 

+ ( ( XKKM1 (3) - VO) ** 2 ) + ( ( XKKIil (5) - ZD) ** 2 ) ) 

RETURN 
END 
C 

SUBROUTINE NOISE (R,P) 

REALMS Y(6) ,X(6) ,S(5) ,R,P,BB,P1 

DATA Y/O . O , . 0228 , . 0668, . 1357, .2743, . 5/ 

DATA X/-3.01 ,-2.0, -1.5, -1.0, -0.6, 0.0/ 

DATA S/43. 8596 ,11. 3636 , 7 . 25689 , 2. 89 1 352 ,2. 65887/ 

BB = 1.0 

PI = R * 317.0 

R = DMOD ( P 1 , BB) 

P = R 
1 = 1 

IFCP.GT.0.5) P = 1.0 - R 

8 IF (P. LT. Y ( 1 + 1 ) ) GO TO 80 
1 = 1 + 1 

GO TO 8 

80 P = <<P - Y ( I ) ) * S(I) + X < I ) ) 

IF (R. GE. 0. 5) P = - P 
RETURN 
END 
C 

SUBROUTINE ADD ( AA , BB , NR , NC , CC) 

REAL*8 AA ( NR , NC ) , BB ( NR , NC) ,CC(NR,NC) 

DO 9 I = 1 , NR 
DO 90 J = 1 , NC 

CC ( I , J ) = AA ( I , J ) + BB ( I , J ) 

90 CONTINUE 

9 CONTINUE 
RETURN 
END 

C 

SUBROUTINE QFIND (KK , XKK , PKK , S IGACC , S IGDI V , S IGCC , A , Q) 
REAL*S XKK (5) ,PKK(5,5) ,0(5,5) , S IGACC , S IGDI V , S IGCC , A 
REAL+8 A2 , A3 , B , C , D , El , E12 , E2 , G 1 , G2 , G3 , SIGAAC , SIGDD I 
REAL*8 SIGCCC , A1 
INTEGER KK 

IF (KK. NE. 1) GO TO 111 
DO 11 1=1 ,5 

DO 110 J = 1 ,5 

0(1, J) = 0.0 
110 CONTINUE 
11 CONTINUE 

S IGACC = S IGACC **2 

0(5,5) = (SIGDIV **2) * (A **2) 

SIGCC = SIGCC **2 
G1 = ( A **2 ) / 2.0 

G2 = G 1 **2 
G3 = A * G 1 
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A2 = A **2 

111 A 1 = XKK (2) **2 + XKK ( 4) **2 

A3 = XKK (2) / DSQRT(Al) 

B = XKK (4) 

C = XKK (4) / DSQRT(Al) 

D = XKK (2) 

El = < A3 ** 2 ) * SIGACC + ( B **2 ) * S1GCC 

El 2 = A3 * C * SIGACC - B * D * SIGCC 

E2 = < C **2 > * SIGACC + ( D **2 ) * SIGCC 

Q<1,1> = El * G2 
Q ( 1 ,2) = G3 * El 
Q< 1 ,3) = E12 * G2 
Q ( 1 , 4) = G3 * E12 
Q (2,2) = A2 * El 
Q (2 ,3) = G3 * E12 
□(2,4) = A2 * El 2 
0(3,3) = G2 * E2 
Q (3 , 4) = G3 * E2 
Q ( 4 , 4) = A2 * E2 
DO 112 I = 1 , 4 

DO 113 J = 1 , I 

Q ( I , J) = Q ( J , I ) 

113 CONTINUE 

112 CONTINUE 
RETURN 
END 

C 

SUBROUTINE SUB ( AA , BB , NR , NC , CC ) 

REAL*8 AA (NR,NC) , BB (NR ,NC) ,CC(NR,NC) 

DO 12 I = 1 , NR 
DO 120 J = 1 , NC 
CC(I,J) = A A ( I , J ) - BB ( I , J ) 

120 CONTINUE 
12 CONTINUE 
RETURN 
END 
C 

SUBROUTINE RECIP ( AA , NN , CC) 

REAL*8 AA (NN,NN) ,DD(5, 10) ,CC(NN,NN) 

DO 14 K = 1 , NN 

DO 140 J = 1 , NN 

DD ( K , J ) = AA ( K , J ) 

140 CONTINUE 
14 CONTINUE 

DO 141 K = 1 , NN 

I = K + NN 
DO 142 J = 6 , 10 
IF(I.NE.J) GO TO 143 
DD ( K , J ) = 1. 

GO TO 142 

143 DD (K , J > = 0. 



102 



142 CONTINUE 
141 CONTINUE 

DO 144 K = 1 , NN 

M = K + 1 
DO 145 J = M , 10 
DD (K , J ) = DD<K,J) / DD(K,K) 

145 CONTINUE 

DD <K ,K) = 1. 

DO 146 L = 1 , NN 

IF(L.EQ.K) GO TO 146 
DO 147 I = 1 , 10 

IF (I. ED. K) GO TO 147 
DD (L , I ) = DD (L , I ) - DD(L,K) 
147 CONTINUE 

DD <L,K) - O. 

146 CONTINUE 
144 CONTINUE 

DO 148 K = 1 , NN 
DO 149 J = 1 , NN 

I = J + NN 
CC (K , J ) = DD (K , I ) 

149 CONTINUE 

1 43 CONTINUE 
RETURN 
END 



* DD ( K , I ) 
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APPENDIX C 

PLOTTING PROGRAM LISTING FOR HP PLOTTER 



^STORAGE: 2 
f DEBUG 
$NOLIST 
C 

PROGRAM PLOTTER 

C 

CHARACTER*40 TITLE 
CHARACTER*35 LEGEND, SUBTITLE 
CHARACTER*25 NAMEX ,NAMEV 
REAL X (245) ,Y(245> 

REAL 0(245) ,P(245) ,R(245) ,S(245) ,T(245) ,U(245) 
INTEGER*2 IC 
DATA IC/O/ 

C 

C USE THESE FOR MULTIPLE ARRAY TRACKING 

C TITLE = 'MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN ' 

TITLE =' MULTIPLE ARRAY ADAPTIVE STRAIGHT RUN' 

C 

C USE THESE FOR SINGLE ARRAY TRACKING 

C TITLE = 'SINGLE ARRAY ADAPTIVE MANEUVERING RUN' 

C TITLE = 'SINGLE ARRAY ADAPTIVE STRAIGHT RUN' 

OPEN ( 5 , F I LE= ' XKK . DAT ' , STATUS* ' OLD ' ) 

DO 32 LENG = 1 , 241 

READ ( 5 , * , END=33) O (LENG) , P (LENG) , R (LENG) , S (LENG) , 
+ T (LENG) ,U (LENG) 

32 CONTINUE 

33 CONTINUE 

LENG = LENG - 1 
CLOSE ( 5 , STATUS* ' KEEP ' ) 

NAMEX = ' X CK/KI (FT) ' 

NAMEY = ' YCK/KU (FT) ' 

SUBTITLE =' 

LEGEND = 'FILTERED ESTIMATE OF TRAJECTORY' 

CALL DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , P , S , LENG , 

+ SUBTITLE) 

OPEN (5,FILE='PKK. DAT' , STATUS* ' OLD ' ) 

DO 34 LENG =1 , 241 

READ ( 5 , * , END=35 ) O (LENG) , P (LENG) , R (LENG) , S (LENG) , 

+ T (LENG) ,U (LENG) 

34 CONTINUE 

35 CONTINUE 

LENG = LENG - 1 
CLOSE ( 5 , STATUS* ' KEEP ' ) 

NAMEX = ' TIME SLOTS' 

NAMEY = ' (FT**2) ' 
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SUBTITLE ='PCK/K3 (1,1) ' 

LEGEND =' FILTERED ERROR COVARIANCE P<K/K> ' 

CALL DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , O , P , LENG , 

+ SUBTITLE) 

NAMEY = ' ( (FT/SEC) **2) ' 

SUBTITLE -'PCK/KD (2,2) ' 

CALL DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , R , LENG , 

+ SUBTITLE) 

NAMEY = ' (FT*"*2) ' 

SUBTITLE ='PCK/K3 (3,3) ' 

CALL DRAWER (TITLE, NAME X , NAMEY , LEGEND , O , S , LENG , 

+ SUBTITLE) 

NAMEY = ' ( (FT/SEC) **2) ' 

SUBTITLE ='PCK/K3 (4,4) ' 

CALL DRAWER (TITLE , NAMEX , NAMEY , LEGEND , O , T , LENG , 

+ SUBTITLE) 

NAMEY = ' <FT**2) ' 

SUBTITLE = 'PCK/KH (5,5) ' 

CALL DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , U , LENG , 

+ SUBTITLE) 

OPEN (5,FILE= ' XKERR. DAT ' , ST ATUS* ' OLD ' ) 

DO 38 LENG = 1 , 241 

READ ( 5 , * , END=39 ) 0 (LENG) , P (LENG) , R (LENG) , S (LENG) 

38 CONTINUE 

39 CONTINUE 

LENG = LENG - 1 
CLOSE (5 , STATUS* ' KEEP ' ) 

NAMEY = ' X ERROR (FT)' 

SUBTITLE = ' 

LEGEND = ' ERROR IN FILTERED ESTIMATE' 

CALL DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , P , LENG , 

+ SUBTITLE) 

NAMEY ='Y ERROR (FT) ' 

CALL DRAWER (TITLE, NAME X , NAMEY , LEGEND , 0 , R , LENG , 

+ SUBTITLE) 

NAMEY ='Z ERROR (FT) ' 

CALL DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , S , LENG , 

+ SUBTITLE) 

OPEN ( 5 , F I LE= ' XKN . DAT ' , STATUS* ' OLD ' ) 

DO 40 LENG = 1 , 241 

READ ( 5 , •* , END=4 1 ) 0 (LENG) , P (LENG) , R (LENG) , S (LENG) , 
+ T (LENG) ,U (LENG) 

40 CONTINUE 

41 CONTINUE 

LENG = LENG - 1 
CLOSE ( 5 , STATUS* ' KEEP ' ) 

NAMEX = ' X CK/N3 (FT) ' 

NAMEY = ' YCK/N3 (FT)' 

LEGEND = 'SMOOTHED ESTIMATE OF TRAJECTORY' 

CALL DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , P , S , LENG , 

+ SUBTITLE) 
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OPEN ( 5 , F I LE= ' PKN . DAT ' , STATUS= ' OLD ' ) 

DO 42 LENG = 1 , 241 

READ (5 , * , END=43) 0 (LENG) ,P (LENG) , R <LENG) , S <LENG) , 
+ * T (LENG) ,U (LENG) 

42 CONTINUE 

43 CONTINUE 

LENG = LENG - 1 
CLOSE ( 5 , STATUS^ ' KEEP ' ) 

NAMEX = ' TIME SLOTS' 

NAMEY = ' (FT**2) ' 

SUBTITLE = ' PCK/N3 (1,1) 

LEGEND = 'SMOOTHED ERROR COVARIANCE P(K/N) ' 

CALL DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , P , LENG , 

+• . SUBTITLE) 

NAMEY ='( (FT/SEC) **2) ' 

SUBTITLE ='PCK/N3 (2,2) ' 

CALL DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND ,Q,R, LENG , 

+ SUBTITLE) 

NAMEY ='(FT**2)' 

SUBTITLE = 'PCK/NI (3,3) ' 

CALL DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , S , LENG , 

+ SUBTITLE) 

NAMEY =' ( (FT /SEC) **2) ' 

SUBTITLE -'PCK/N3 (4,4) ' 

CALL DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , T , LENG , 

+ SUBTITLE) 

NAMEY =' (FT**2) ' 

SUBTITLE. = 'PCK/NI (5,5) ' 

CALL DRAWER ( TITLE , NAMEX , NAMEY , LEGEND , 0 , U , LENG , 

+ SUBTITLE) 

OPEN <5,FILE=' XSERR.DAT' , ST ATUS= ' OLD ' ) 

DO 44 LENG = 1 , 241 

READ ( 5 , * , END=45 ) 0 (LENG) , P (LENG) , R (LENG) , S (LENG) 

44 CONTINUE 

45 CONTINUE 

LENG = LENG - 1 
CLOSE ( 5 , STATUS= ' KEEP ' ) 

NAMEY = ' X ERROR (FT)' 

SUBTITLE =' 

LEGEND =' ERROR IN SMOOTHED ESTIMATE' 

CALL DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , P , LENG , 

+ SUBTITLE) 

NAMEY ='Y ERROR (FT) ' 

CALL DRAWER (TITLE, NAMEX , NAMEY, LEGEND, 0,R, LENG, 

+ SUBTITLE) 

NAMEY ='Z ERROR (FT)' 

CALL DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , S , LENG , 

+ SUBTITLE) 

STOP 

END 

SUBROUT I NE DRAWER (TITLE, NAME X , NAMEY , LEGEND , X , Y , 
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LENG, SUBTITLE) 



CHARACTER*40 TITLE 
CHARACTER*35 LEGEND , SUBTITLE 
CHARACTER*25 NAMEX , NAMEY 
REAL X (245) , Y (245) 

INTEGERS IC 
DATA IC/O/ 



CALL ED 
CALL CUP(1 ,0) 

CALL PLOTS (0,9600,30) 

CALL SYMBOL ( 2 . O , 6 . 65 , . 20 , T I TLE ,0.0,40) 

CALL SYMBOL (2.0,6. 25, . 175 , LEGEND , O. 0 , 35) 

C 

C USE THIS FOR NOISELESS TRACKING 

C CALL SYMB0L(6. 84,6. 25, . 175, 'WITHOUT NOISE ' , O. 0 , 1 3) 

C 

C USE THIS FOR NOISY TRACKING 

CALL SYMB0L(6. 84, 6.25, . 175, ' WITH NOISE ' ,0. 0 , 13) 

C 



CALL SYMBOL (1.60, 2. 45, . 20, SUBTITLE, 90. 0,35) 

CALL PLOT ( 1 . 00, 1 . 00,-3) 

CALL PLOT ( 8 . 0 , 0. 0 , 3) 

CALL PLOT (8. 0,6. 0,2) 

CALL PLOT (0.0,6. 0,2) 

CALL PLOT (0.0, 0.0, 2) 

CALL PLOT (8. 0 , O. 0 , 2) 

CALL SCALE (X, 6. 00, LENG, 1) 

CALL SCALE ( Y , 3 . 00 , LENG , 1 ) 

CALL STAX IS ( . 1 80 , . 20 , . 1 5 , . 1 1 2 , -1 ) 

CALL AX IS (1.5, 1.5, NAMEX, -13, 6. 00, 00. ,X(LENG+1> , 
+ X (LENG+2) ) 

CALL STAXIS(. 15, .20, . 1 11 , . 1 12,2) 

CALL AXIS (1 .5, 1 . 5, NAMEY, 13,3. 00,90. , Y (LENG+1 ) , 

+ Y (LENG+2)) 



CALL PLOT (1.50, 1.50, -3) 

CALL LINE(X, Y, LENG, 1 ,0,3) 

CALL PL0T(0. 0,0. 0,999) 

RETURN 

END 

SUBROUTINE ED 
CHARACTER* 1 C 1 , C2 , C3 , C4 
INTEGER*2 IC(4) 

EQUIVALENCE (Cl , IC ( 1 ) ) , (C2 , IC (2) ) , (C3 , IC (3) ) , 
+ (C4 , IC (4) ) 

DATA IC/16#1B, 16#5B, 16#32, 16#4A/ 

WRITE ( * , 1 ) C1,C2,C3,C4 
1 FORMAT (1X,4A1) 

RETURN 

END 

SUBROUTINE CUP(N,M) 
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CHARACTER* 1 C 1 , C2 , C5 , C8 , LC (5) 

CHARACTER*5 CBUFF 
INTEGER*2 IC(4) 

EQUIVALENCE (Cl , IC ( 1 ) ) , (C2, IC (2) ) , (C5 , IC (3) ) , 

+ (CS,IC(4> ), (CBUFF, LC(1> > 

DATA IC/16#1B, 16#5B, 16#3B, 16#6<S/ 

L= 1 0000+ 1 00*N+M 
WR ITE (CBUFF , 2) L 
FORMAT (15) 

WRITE ( * , 1 ) Cl , C2 , LC (2) ,LC(3) ,C5,LC(4) ,LC(5) ,C8 
FORMAT ( IX, 8A1 , \) 

RETURN 

END 
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APPENDIX D 

PLOTTING PROGRAM LISTING FOR MONITOR 



^STORAGE: 2 
•SDEBUG 
•SNOLIST 
C 

PROGRAM MONITOR 
C 

CHARACTER*40 TITLE 
CHARACTER*35 LEGEND 
CHARACTER*25 NAMEX , NAMEY 
REAL X (245) ,Y(245) 

REAL 0(245) ,P(245) ,R(245) ,S(245) ,T(245) ,U(245) 
INTEGERS IC 
DATA IC/O/ 

C 

C USE THESE FOR MULTIPLE ARRAY TRACKING 

C TITLE = 'MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN ' 

TITLE = 'MULTIPLE ARRAY ADAPTIVE STRAIGHT RUN' 

C 

C USE THESE FOR SINGLE ARRAY TRACKING 

C TITLE = 'SINGLE ARRAY ADAPTIVE MANEUVERING RUN ' 

C TITLE = 'SINGLE ARRAY ADAPTIVE STRAIGHT RUN' 

OPEN (5,FILE='XKK. DAT' , STATUS= ' OLD ' ) 

DO 32 LENG =1 ,241 

READ ( 5 , * , END=33 ) 0 (LENG) , P (LENG) , R (LENG) , S (LENG) , 
+ T (LENG) ,U (LENG) 

32 CONTINUE 

33 CONTINUE 

LENG = LENG - 1 
CLOSE ( 5 , ST ATUS= ' KEEP ' ) 

NAMEX ='XCK/K] (FT) ' 

NAMEY ='YCK/KI1 (FT)' 

LEGEND =' FILTERED ESTIMATE OF TRAJECTORY' 

CALL DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , P , S , LENG ) 

OPEN (5,FILE= 'PKK. DAT ' , ST ATUS= ' OLD ' ) 

DO 34 LENG =1 ,241 

READ ( 5 , * , END=35 ) 0 (LENG) , P (LENG) ,R (LENG) , S (LENG) , 

+ T (LENG) ,U (LENG) 

34 CONTINUE 

35 CONTINUE 

LENG = LENG - 1 
CLOSE ( 5 , ST ATUS= ' KEEP ' ) 

NAMEX = ' T I ME SLOTS ' 

NAMEY = ' PCK/KD (1,1) ' 

LEGEND = 'FILTERED ERROR COVARIANCE P(K/K>* 

CALL DRAWER ( T I TLE , NAME X , NAMEY , LEGEND , 0 , P , LENG ) 
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NAMEY = ' PCK/KI (2,2) ' 

CALL DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , R , LENG ) 
NAMEY = ' PCK/KD (3,3) ' 

CALL DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , S , LENG ) 
NAMEY ='PCK/K 1 (4,4) ' 

CALL DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , T , LENG ) 
NAMEY = 'PCK/K: (5,5) ' 

CALL DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , U , LENG ) 
OPEN (5,FILE= ' XKERR. DAT ’ , ST ATUS= ' OLD ' ) 

DO 38 LENG = 1 , 241 

READ (5,*,END=39) 0 (LENG) , P (LENG) , R (LENG) , S (LENG) 

38 CONTINUE 

39 CONTINUE 

LENG = LENG - 1. 

CLOSE ( 5 , ST ATUS= ' KEEP ’ ) 

NAMEY = ’ X ERROR (FT)' 

LEGEND =' ERROR IN FILTERED ESTIMATE' 

CALL DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , P , LENG ) 
NAMEY ='Y ERROR (FT) ' 

CALL DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , R , LENG ) 
NAMEY = ' Z ERROR (FT) ' 

CALL DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , S , LENG , 
OPEN (5 , F I LE= ' XKN. DAT ' , ST ATUS= ' OLD ' ) 

DO 40 LENG = 1 , 241 

READ ( 5 , * , END=4 1 ) 0 (LENG) , P (LENG) , R (LENG) , S (LENG) , 
+ T (LENG) ,U (LENG) 

40 CONTINUE 

41 CONTINUE 

LENG = LENG - 1 
CLOSE ( 5 , STATUS= ' KEEP ' ) 

NAMEX =‘XCK/NI (FT) ' 

NAMEY = ' YCK/N3 (FT) ' 

LEGEND =' SMOOTHED ESTIMATE OF TRAJECTORY' 

CALL DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , P , S , LENG ) 
OPEN ( 5 , F I LE= ' PKN . DAT ' , ST ATUS= ' OLD ' ) 

DO 42 LENG = 1 , 241 

READ (5,*,END=43) 0 (LENG) , P (LENG) , R (LENG) , S (LENG) , 
+ T (LENG) ,U (LENG) 

42 CONTINUE 

43 CONTINUE 

LENG = LENG - 1 
CLOSE ( 5 , STATUS= ' KEEP ' ) 

NAMEX = ' TIME SLOTS' 

NAMEY ='PCK/N3 (1,1) ' 

LEGEND =' SMOOTHED ERROR COVARIANCE P(K/N> ' 

CALL DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , P , LENG ) 
NAMEY = ' PCK/NI (2,2) ' 

CALL DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , R , LENG ) 
NAMEY ='PCK/N3 (3,3) ' 

CALL DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , S , LENG ) 
NAMEY = ' PCK/N3 (4,4) ' 
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CALL DRAWER ( T I TLE , NAMEX , NAME Y , LEGEND , O , T , LENG ) 

NAMEY ='PCK/N: (5,5) ' 

CALL DRAWER (TITLE, NAMEX , NAMEY , LEGEND , O , U , LENG ) 

OPEN ( 5 , F I LE= ' XSERR . DAT ' , ST ATUS= ' OLD ' ) 

DO 44 LENG = 1 , 241 

READ (5,*,END=45) O (LENG) , P (LENG) , R (LENG) , S (LENG) 

44 CONTINUE 

45 CONTINUE 

LENG = LENG - 1 
CLOSE < 5 , ST ATUS= ' KEEP ' ) 

NAMEY = ' X ERROR (FT)' 

LEGEND =' ERROR IN SMOOTHED ESTIMATE' 

. CALL DRAWER (TITLE, NAMEX , NAMEY, LEGEND, 0,P, LENG) 

NAMEY = ' Y ERROR (FT) ' 

CALL DRAWER ( T I TLE , NAME X , NAMEY , LEGEND , 0 , R , LENG ) 

NAMEY ='Z ERROR (FT)' 

CALL DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , O , S , LENG ) 

STOP 

END 

SUBROUT I NE DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , X , Y , LENG ) 

CHARACTER*40 TITLE 

CHARACTER*35 LEGEND 

CHARACTER+25 NAMEX, NAMEY 

REAL X (245) , Y (245) 

INTEGER+2 IC 
DATA IC/O/ 



C 

CALL ED 
CALL CUP(1 ,0) 

CALL PLOTS (0,99,99) 

CALL SYMBOL (0.5, 5. 15 , . 20 , T ITLE , 0. 0 , 40 ) 

CALL SYMBOL (1 . 04,4.75,. 1 75 , LEGEND , 0 . 0 , 35) 

C 

C USE THIS FOR NOISELESS TRACKING 

C CALL SYMB0L(5.3G,4.75, . 175, 'WITHOUT NOISE ', O. 0 , 13) 

C 

C USE THIS FOR NOISY TRACKING 

CALL SYMBOL (5. 3S ,4.75,. 175 , WITH NOISE ', 0. 0 , 13) 

C 



CALL PLOT (1.00, 1.00, -3) 

CALL SCALE ( X , 6 . 00 , LENG , 1 ) 

CALL SCALE (Y, 3. 00, LENG, 1) 

CALL STAX IS (. 1 SO , . 20 , . 15 , . 1 12 , - 1 ) 

CALL AX IS (O. ,0. , NAMEX, -13, 6 . 00, 00. ,X (LENG+1) 
+ X (LENG+2) ) 

CALL STAXIS(. 15, .20,. 111,. 112,2) 

CALL AX IS (O. ,0. , NAMEY, 13,3.00,90. ,Y (LENG+1) , 
+ Y( LENG+2)) 

CALL LINE (X,Y, LENG, 1 ,0,3) 

CALL PL0T(0. 0,0. 0,999) 

RETURN 



1 1 1 



END 

SUBROUTINE ED 
CHARACTER* 1 C 1 , C2 , C3 , C4 
INTEGER*2 IC<4) 

EQUIVALENCE (C 1 , IC ( 1 ) ) , <C2 , I C ( 2) ) , <C3 , IC ( 3) ) , 

+ (C4 , IC (4) ) 

DATA IC/ 16# IB , 16#5B , 16#32 , 16#4A/ 

WRITE!*, 1) C1,C2,C3,C4 
1 FORMAT (1X,4A1) 

RETURN 

END 

SUBROUTINE CUP(N,M>- 
CHARACTER* 1 C 1 , C2 , C5 , C8 , LC ( 5) 

CHARACTER*5 CBUFF 
INTEGER*2 IC(4) 

EQUIVALENCE (Cl , IC < 1 > > , <C2, IC (2) ) , (C5,IC<3) ) , 

+ (C8,IC(4) ), (CBUFF, LC(1) ) 

DATA IC/16#1B, 16#5B, 16#3B, 16#66/ 

L= 1 0000+ 1 00*N+M 
WRITE (CBUFF, 2) L 
2 FORMAT (15) 

WRITE!*, 1) C1,C2,LC(2> ,LC(3) ,C5,LC(4) ,LC(5) ,CS 
1 FORMAT ( 1 X,8A1 ,\) 

RETURN 

END 
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APPEN DIX E 
BATCH FILES 

A. LISTING OF AUTOEXEC.BAT FILE ON OPERATING SYSTEM DISK 
ECHO OFF 

GRAPHICS 

TIMER/S 

COPY A: RUN. BAT C: 

COPY A: KEDIT.EXE C:/V 
COPY A: PROFILE. KED C: /V 
C: 

RUN 

B. LISTING OF RUN. BAT FILE ON VIRTUAL DISK(C) 

ECHO. Insert the disk, which has the source -file of the 
ECHO, sequential extended Kalman filter and Smoothing, 
ECHO, into drive A 
PAUSE 

COPY A: THESIS. FOR C: 

KEDIT C: THESIS. FOR 
COPY C: THESIS. FOR A: 

ERASE C: KEDIT.EXE 
ERASE C: PROFILE. KED 

ECHO. Insert the disk, which has PROFORT.EXE and 
ECHO. LINK.EXE, into drive A, and the disk, which has 
ECHO. PROFORT. LIB into drive B. 

PAUSE 

A: PROFORT THESIS /L /E 

A : L I NK THES IS,, NULL , PROFORT 

ERASE C: THESIS. FOR 

ERASE C: THESIS. OBJ 

THESIS 

ECHO. Insert the disk, which has the source file of the 
ECHO, sequential extended Kalman filter and Smoothing, 
ECHO, into drive A, and the disk labeled ‘‘DATA'' into 
ECHO, drive B. 

PAUSE 

COPY C: THESIS. EXE A: 

COPY C: *. DAT B: 

ERASE C: *-. *■ 

ECHO. Insert the operating system disk into drive A, and 
ECHO, the disk, which has the plotting routine source 
ECHO, file into drive B. 

PAUSE 

COPY AjKEDIT.EXE C: 

COPY A: PROFILE. KED C: 

COPY B: GRAPH. FOR C: 

KEDIT C: GRAPH. FOR 
COPY C: GRAPH. FOR B: 
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ERASE C: KEDIT.EXE 
ERASE C: PROFILE. KED 

ECHO. Insert the disk, which has F0R1.EXE and PAS2.EXE 
ECHO, into drive A, and the disk, which has PLQT88.LIB 
ECHO, into drive B. 

PAUSE 

A: FORI GRAPH; 

A: PAS2 

ECHO. Insert the disk, which has FORTRAN. LIB, MATH. LIB 
ECHO, and LINK. EXE into drive A. 

PAUSE 

A : L I NK GRAPH , , NULL , B : PL0TT88+A : FORTRAN+A : MATH 
ECHO. Insert the disk, which has the plotting source 
ECHO, -file into drive A and the data disk into drive B. 
PAUSE 

COPY C: GRAPH. EXE A: 

ERASE C: GRAPH. FOR 
ERASE C: GRAPH. OBJ 
COPY B: *. DAT C: 

GRAPH 
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